#include "MBTB_JointR.hpp"
#include "NewtonEulerJointR.hpp"
#include <boost/math/quaternion.hpp>
#include "op3x3.h"
#include "RotationQuaternion.hpp"
//#define MBTB_JOINTR_DEBUG

MBTB_JointR::MBTB_JointR()
{
  _M.reset(new SimpleMatrix(6,6));
  _F.reset(new SiconosVector(6));
}

/*
 *
 *
 * ML_A_abs = M_FA_A + M_FB_A = F2/\BA
 * the system is :
 * FL=F1+F2
 * F1.e0 = F2.e0
 * ML_A_abs . ei = F2 /\ BA .ei for i=1,2
 *
 */

void MBTB_JointR::computeEquivalentForces()
{
  if(!_G0C1)
    return;
  double q1=_ds1->q()->getValue(3);
  double q2=_ds1->q()->getValue(4);
  double q3=_ds1->q()->getValue(5);
  double q4=_ds1->q()->getValue(6);
  ::boost::math::quaternion<double>    quattrf(q1,q2,q3,q4);
  ::boost::math::quaternion<double>    cquattrf(q1,-q2,-q3,-q4);
  ::boost::math::quaternion<double>    quatbuff;
  SP::SiconosVector Blambda=_jointR->contactForce();
  SiconosVector FL(3);
  FL.setValue(0,Blambda->getValue(0));
  FL.setValue(1,Blambda->getValue(1));
  FL.setValue(2,Blambda->getValue(2));

  SiconosVector ML_G(3);
  SiconosVector ML_G_abs(3);
  ML_G.setValue(0,Blambda->getValue(3));
  ML_G.setValue(1,Blambda->getValue(4));
  ML_G.setValue(2,Blambda->getValue(5));
  SP::SiconosVector spML_G_abs(new SiconosVector(3));
  *spML_G_abs = ML_G;
  changeFrameBodyToAbs(_ds1->q(),spML_G_abs);
  ML_G_abs = * spML_G_abs;
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces Blambda\n");
  Blambda->display();
  printf("MBTB_JointR::computeEquivalentForces ML_G_abs (in abs frame):\n");
  ML_G_abs.display();
#endif

  SiconosVector AB0(3);
  AB0=(*_G0C2)-(*_G0C1);
  ::boost::math::quaternion<double>    quatAB0(0,AB0.getValue(0),AB0.getValue(1),AB0.getValue(2));
  quatbuff = quattrf*quatAB0*cquattrf;
  SiconosVector AB(3);
  AB.setValue(0,quatbuff.R_component_2());
  AB.setValue(1,quatbuff.R_component_3());
  AB.setValue(2,quatbuff.R_component_4());
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces AB0:\n");
  AB0.display();
  printf("MBTB_JointR::computeEquivalentForces AB:\n");
  AB.display();
#endif

  ::boost::math::quaternion<double>    quatG0C1(0,_G0C1->getValue(0),_G0C1->getValue(1),_G0C1->getValue(2));
  quatbuff = quattrf*quatG0C1*cquattrf;
  SiconosVector GA(3);
  GA.setValue(0,quatbuff.R_component_2());
  GA.setValue(1,quatbuff.R_component_3());
  GA.setValue(2,quatbuff.R_component_4());
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces GA:\n");
  GA.display();
#endif
  SiconosVector ML_A_abs(3);
  SiconosVector GA_FL(3);
  cross_product(GA,FL,GA_FL);
  ML_A_abs=ML_G_abs - GA_FL;
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces GA_FL:\n");
  GA_FL.display();
  printf("MBTB_JointR::computeEquivalentForces ML_A_abs :\n");
  ML_A_abs.display();
#endif
  double normAB=1.0/AB.norm2();
  double e0_x =normAB*AB.getValue(0);
  double e0_y =normAB*AB.getValue(1);
  double e0_z =normAB*AB.getValue(2);
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces e0_x . ML_A_abs (must be zero):%e\n",
         e0_x*ML_A_abs.getValue(0)+e0_y*ML_A_abs.getValue(1)+e0_z*ML_A_abs.getValue(2));
#endif
  double  e1_x,e1_y,e1_z,e2_x,e2_y,e2_z;
  int info = orthoBaseFromVector(&e0_x,&e0_y,&e0_z,&e1_x,&e1_y,&e1_z,&e2_x,&e2_y,&e2_z);
  if(info)
  {
    std::cout << "something wrong happened in  orthoBaseFromVector"<<std::endl;
  }
  /*
   * ML_A_abs = M_FA_A + M_FB_A = F2/\BA
   * the system is :
   * FL=F1+F2
   * F1.e0 = F2.e0
   * ML_A_abs . ei = F2 /\ BA .ei for i=1,2
   *
   */
  /*Fill _M*/
  _M->zero();
  //FL= F1+F2.
  _M->setValue(0,0,1);
  _M->setValue(0,3,1);
  _M->setValue(1,1,1);
  _M->setValue(1,4,1);
  _M->setValue(2,2,1);
  _M->setValue(2,5,1);
  /*F1.e0=F2.e0*/
  _M->setValue(3,0,e0_x);
  _M->setValue(3,1,e0_y);
  _M->setValue(3,2,e0_z);
  _M->setValue(3,3,-e0_x);
  _M->setValue(3,4,-e0_y);
  _M->setValue(3,5,-e0_z);

  /*
   *             F2x  BAx    F2y*BAz-F2z*BAy
   * FB/\BA . ei=F2y/\BAy.ei=F2z*BAx-F2x*BAz .ei = F2x*(BAy *ei_z-BAz*ei_y)+F2y*(BAz*ei_x-BAx*ei_z)+F2z*(BAx*ei_y-BAy*ei_x)
   *             F2z  BAz    F2x*BAy-F2y*BAx
   *
   */
  _M->setValue(4,3,-AB.getValue(1) *e1_z+AB.getValue(2)*e1_y);
  _M->setValue(4,4,-AB.getValue(2) *e1_x+AB.getValue(0)*e1_z);
  _M->setValue(4,5,-AB.getValue(0) *e1_y+AB.getValue(1)*e1_x);

  _M->setValue(5,3,-AB.getValue(1) *e2_z+AB.getValue(2)*e2_y);
  _M->setValue(5,4,-AB.getValue(2) *e2_x+AB.getValue(0)*e2_z);
  _M->setValue(5,5,-AB.getValue(0) *e2_y+AB.getValue(1)*e2_x);
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces the sytem M is:\n");
  _M->display();
#endif
  _F->setValue(0,FL.getValue(0));
  _F->setValue(1,FL.getValue(1));
  _F->setValue(2,FL.getValue(2));
  _F->setValue(3,0);
  _F->setValue(4,e1_x * ML_A_abs.getValue(0) + e1_y * ML_A_abs.getValue(1) +e1_z * ML_A_abs.getValue(2));
  _F->setValue(5,e2_x * ML_A_abs.getValue(0) + e2_y * ML_A_abs.getValue(1) +e2_z * ML_A_abs.getValue(2));
#ifdef MBTB_JOINTR_DEBUG
  printf("MBTB_JointR::computeEquivalentForces Mx=b, b:");
  _F->display();
#endif

  /*Solve the system.*/
  try
  {
    _M->PLUFactorizationInPlace();
    _M->PLUSolve(*_F);
#ifdef MBTB_JOINTR_DEBUG
    printf("MBTB_JointR::computeEquivalentForces Forces equivalent:");
    _F->display();
    printf("MBTB_JointR::computeEquivalentForces checking ML_G_abs = MF1_G+MF2_G:");
    SiconosVector Maux1(3),Maux2(3),F1(3),F2(3),GC1(3),GC2(3),dif(3);
    F1.setValue(0,_F->getValue(0));
    F1.setValue(1,_F->getValue(1));
    F1.setValue(2,_F->getValue(2));
    F2.setValue(0,_F->getValue(3));
    F2.setValue(1,_F->getValue(4));
    F2.setValue(2,_F->getValue(5));
    //FL=F1+F2
    dif = FL-F1-F2;
    printf("MBTB_JointR::computeEquivalentForces  FL-F1-F2(must be zero):\n");
    dif.display();
    //MF1_G=F1/\C1G
    cross_product(GA,F1,Maux1);
    ::boost::math::quaternion<double>    quatG0C2(0,_G0C2->getValue(0),_G0C2->getValue(1),_G0C2->getValue(2));
    quatbuff = quattrf*quatG0C2*cquattrf;
    SiconosVector GB(3);
    GB.setValue(0,quatbuff.R_component_2());
    GB.setValue(1,quatbuff.R_component_3());
    GB.setValue(2,quatbuff.R_component_4());
    cross_product(GB,F2,Maux2);
    dif=Maux1+Maux2;
    printf("MBTB_JointR::computeEquivalentForces  momentum (must be ML_G_abs):\n");
    dif.display();
    dif=Maux1+Maux2 - ML_G_abs;
    printf("MBTB_JointR::computeEquivalentForces  dif momentum(must be zero):\n");
    dif.display();
#endif
  }
  catch(const std::exception& e)
  {
    printf("MBTB_JointR: exception caught.\n");
  }

}


#ifndef STAN_MATH_PRIM_SCAL_PROB_FRECHET_LPDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_FRECHET_LPDF_HPP

#include <boost/random/weibull_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/size_zero.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/meta/VectorBuilder.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <cmath>

namespace stan {
namespace math {

// Frechet(y|alpha, sigma)     [y > 0;  alpha > 0;  sigma > 0]
// FIXME: document
template <bool propto, typename T_y, typename T_shape, typename T_scale>
typename return_type<T_y, T_shape, T_scale>::type frechet_lpdf(
    const T_y& y, const T_shape& alpha, const T_scale& sigma) {
  static const char* function = "frechet_lpdf";
  typedef typename stan::partials_return_type<T_y, T_shape, T_scale>::type
      T_partials_return;

  using std::log;

  if (size_zero(y, alpha, sigma))
    return 0.0;

  T_partials_return logp(0.0);
  check_positive(function, "Random variable", y);
  check_positive_finite(function, "Shape parameter", alpha);
  check_positive_finite(function, "Scale parameter", sigma);
  check_consistent_sizes(function, "Random variable", y, "Shape parameter",
                         alpha, "Scale parameter", sigma);

  if (!include_summand<propto, T_y, T_shape, T_scale>::value)
    return 0.0;

  scalar_seq_view<T_y> y_vec(y);
  scalar_seq_view<T_shape> alpha_vec(alpha);
  scalar_seq_view<T_scale> sigma_vec(sigma);
  size_t N = max_size(y, alpha, sigma);

  VectorBuilder<include_summand<propto, T_shape>::value, T_partials_return,
                T_shape>
      log_alpha(length(alpha));
  for (size_t i = 0; i < length(alpha); i++)
    if (include_summand<propto, T_shape>::value)
      log_alpha[i] = log(value_of(alpha_vec[i]));

  VectorBuilder<include_summand<propto, T_y, T_shape>::value, T_partials_return,
                T_y>
      log_y(length(y));
  for (size_t i = 0; i < length(y); i++)
    if (include_summand<propto, T_y, T_shape>::value)
      log_y[i] = log(value_of(y_vec[i]));

  VectorBuilder<include_summand<propto, T_shape, T_scale>::value,
                T_partials_return, T_scale>
      log_sigma(length(sigma));
  for (size_t i = 0; i < length(sigma); i++)
    if (include_summand<propto, T_shape, T_scale>::value)
      log_sigma[i] = log(value_of(sigma_vec[i]));

  VectorBuilder<include_summand<propto, T_y, T_shape, T_scale>::value,
                T_partials_return, T_y>
      inv_y(length(y));
  for (size_t i = 0; i < length(y); i++)
    if (include_summand<propto, T_y, T_shape, T_scale>::value)
      inv_y[i] = 1.0 / value_of(y_vec[i]);

  VectorBuilder<include_summand<propto, T_y, T_shape, T_scale>::value,
                T_partials_return, T_y, T_shape, T_scale>
      sigma_div_y_pow_alpha(N);
  for (size_t i = 0; i < N; i++)
    if (include_summand<propto, T_y, T_shape, T_scale>::value) {
      const T_partials_return alpha_dbl = value_of(alpha_vec[i]);
      sigma_div_y_pow_alpha[i]
          = pow(inv_y[i] * value_of(sigma_vec[i]), alpha_dbl);
    }

  operands_and_partials<T_y, T_shape, T_scale> ops_partials(y, alpha, sigma);
  for (size_t n = 0; n < N; n++) {
    const T_partials_return alpha_dbl = value_of(alpha_vec[n]);
    if (include_summand<propto, T_shape>::value)
      logp += log_alpha[n];
    if (include_summand<propto, T_y, T_shape>::value)
      logp -= (alpha_dbl + 1.0) * log_y[n];
    if (include_summand<propto, T_shape, T_scale>::value)
      logp += alpha_dbl * log_sigma[n];
    if (include_summand<propto, T_y, T_shape, T_scale>::value)
      logp -= sigma_div_y_pow_alpha[n];

    if (!is_constant_struct<T_y>::value) {
      const T_partials_return inv_y_dbl = value_of(inv_y[n]);
      ops_partials.edge1_.partials_[n]
          += -(alpha_dbl + 1.0) * inv_y_dbl
             + alpha_dbl * sigma_div_y_pow_alpha[n] * inv_y_dbl;
    }
    if (!is_constant_struct<T_shape>::value)
      ops_partials.edge2_.partials_[n]
          += 1.0 / alpha_dbl
             + (1.0 - sigma_div_y_pow_alpha[n]) * (log_sigma[n] - log_y[n]);
    if (!is_constant_struct<T_scale>::value)
      ops_partials.edge3_.partials_[n] += alpha_dbl / value_of(sigma_vec[n])
                                          * (1 - sigma_div_y_pow_alpha[n]);
  }
  return ops_partials.build(logp);
}

template <typename T_y, typename T_shape, typename T_scale>
inline typename return_type<T_y, T_shape, T_scale>::type frechet_lpdf(
    const T_y& y, const T_shape& alpha, const T_scale& sigma) {
  return frechet_lpdf<false>(y, alpha, sigma);
}

}  // namespace math
}  // namespace stan
#endif


/*
 * 	surgeme_server.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2017-11-06
 *
 *	Separated ROS node to support preempted irob_msgs/Surgeme actions.
 *  Contains the implementation of surgemes, like
 *	grasp, release, cut, nav_to_pos, etc.
 *  Handles instrument info to decide is the surgeme is executable with the
 *  installed instrument, and calculate using the length of jaws.
 *
 */

#ifndef SURGAME_SERVER_HPP_
#define SURGAME_SERVER_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float32.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <irob_utils/pose.hpp>
#include <irob_utils/trajectory.hpp>
#include <irob_utils/utils.hpp>
#include <irob_general_robot/robot_client.hpp>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <irob_msgs/ToolPoseStamped.h>

#include <irob_msgs/SurgemeAction.h>

namespace saf {

class SurgemeServer {

public:

  struct SurgemeSetting {
    double jaw_open_angle;
    double jaw_closed_angle;
    Eigen::Vector3d t;	// translation

    friend std::ostream& operator<<(std::ostream&, const SurgemeSetting&);
  };

protected:
  RobotClient arm;
  ros::NodeHandle nh;

  // Action servers
  actionlib::SimpleActionServer<irob_msgs::SurgemeAction> as;

  static const double DEFAULT_LOOP_RATE;					// Hz

public:
  SurgemeServer(ros::NodeHandle, std::string, double);		// dt
  ~SurgemeServer();

  // Callbacks

  void surgemeActionCB(
      const irob_msgs::SurgemeGoalConstPtr &);

  Pose getPoseCurrent();
  std::string getArmName();

protected:

  // Methods for surgeme execution
  void stop();

  void nav_to_pos(Pose ,std::vector<Pose>, InterpolationMethod, double);

  void grasp(Pose, Pose, double, double, std::vector<Pose>,
             InterpolationMethod,
             double, double);

  void cut(Pose, Pose, double, std::vector<Pose>,
           InterpolationMethod,
           double, double);

  void push(Pose, Pose, Eigen::Vector3d, std::vector<Pose>,
            InterpolationMethod,
            double, double);

  void dissect(Pose, Pose, Eigen::Vector3d,double, std::vector<Pose>,
               InterpolationMethod,
               double, double);

  void release(Pose, double,
               double, double);

  void place(Pose, Pose, double, std::vector<Pose>, InterpolationMethod,
             double);

  void manipulate(Eigen::Vector3d,
                  double);

  bool waitForActionDone(std::string);
  bool handleActionState(std::string, bool = false);
  bool isAbleToDoSurgeme(int);
  irob_msgs::InstrumentJawPart findInstrumentJawPartForSurgeme(int);
  SurgemeSetting calcSurgemeSetting(int, irob_msgs::InstrumentJawPart,
                                    Eigen::Quaternion<double>, double, double = 1.0);

};

}
#endif /* SURGAME_SERVER_HPP_ */


// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_ENVELOPE_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_ENVELOPE_GEOGRAPHIC_HPP

#include <type_traits>

#include <boost/geometry/strategy/geographic/envelope.hpp>
#include <boost/geometry/strategy/geographic/envelope_segment.hpp>

#include <boost/geometry/strategy/geographic/expand_segment.hpp> // TEMP

#include <boost/geometry/strategies/envelope/spherical.hpp>
#include <boost/geometry/strategies/expand/geographic.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace envelope
{

template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public strategies::expand::geographic<FormulaPolicy, Spheroid, CalculationType>
{
    using base_t = strategies::expand::geographic<FormulaPolicy, Spheroid, CalculationType>;

public:
    geographic()
        : base_t()
    {}

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    template <typename Geometry, typename Box>
    static auto envelope(Geometry const&, Box const&,
                         typename util::enable_if_point_t<Geometry> * = nullptr)
    {
        return strategy::envelope::spherical_point();
    }

    template <typename Geometry, typename Box>
    static auto envelope(Geometry const&, Box const&,
                         typename util::enable_if_multi_point_t<Geometry> * = nullptr)
    {
        return strategy::envelope::spherical_multipoint();
    }

    template <typename Geometry, typename Box>
    static auto envelope(Geometry const&, Box const&,
                         typename util::enable_if_box_t<Geometry> * = nullptr)
    {
        return strategy::envelope::spherical_box();
    }

    template <typename Geometry, typename Box>
    auto envelope(Geometry const&, Box const&,
                  typename util::enable_if_segment_t<Geometry> * = nullptr) const
    {
        return strategy::envelope::geographic_segment
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry, typename Box>
    auto envelope(Geometry const&, Box const&,
                  typename util::enable_if_polysegmental_t<Geometry> * = nullptr) const
    {
        return strategy::envelope::geographic
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }
};

namespace services
{

template <typename Geometry, typename Box>
struct default_strategy<Geometry, Box, geographic_tag>
{
    using type = strategies::envelope::geographic<>;
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::envelope::geographic_segment<FP, S, CT> >
{
    static auto get(strategy::envelope::geographic_segment<FP, S, CT> const& s)
    {
        return strategies::envelope::geographic<FP, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::envelope::geographic<FP, S, CT> >
{
    static auto get(strategy::envelope::geographic<FP, S, CT> const& s)
    {
        return strategies::envelope::geographic<FP, S, CT>(s.model());
    }
};

} // namespace services

}} // namespace strategies::envelope

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_ENVELOPE_GEOGRAPHIC_HPP


#include <boost/thread.hpp>

#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/transform_datatypes.h>

#include <ground_based_detector/humanArray.h>
#include <ground_based_detector/human.h>
#include <RegionNormalizer.h>

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> CloudT;
typedef typename pcl::PointCloud<PointT>::Ptr CloudTPtr;
typedef typename pcl::PointCloud<PointT>::ConstPtr CloudTConstPtr;

CloudTPtr cloud(new CloudT);
boost::mutex cloud_mutex;
bool new_cloud = false;
ros::Publisher* posesPublisher_ptr;

void cloud_cb_ (const sensor_msgs::PointCloud2ConstPtr& callback_cloud) {
	cloud_mutex.lock();
	pcl::fromROSMsg(*callback_cloud, *cloud);
	new_cloud = true;
	cloud_mutex.unlock();
}

inline double getROSYaw(const pcl::Normal& vector) {
  double angle = atan2(vector.normal_x, vector.normal_z);
  return isnan(angle) ? 0 : -angle;
}

void human_cb_ (const ground_based_detector::humanArrayConstPtr& humans) {
	if(!new_cloud) return;
	geometry_msgs::PoseArray poses;
	poses.header.frame_id = "/camera_depth_frame";

	unsigned int k = 0;
	for(std::vector<ground_based_detector::human>::const_iterator human = humans->humans.begin(); human != humans->humans.end(); ++human) {
		geometry_msgs::Pose pose;
		pose.position.x = human->ttop_z;
		pose.position.y = -human->ttop_x;
		pose.position.z = -human->ttop_y - 0.4;

		RegionNormalizer<PointT> normalizer(k++, Eigen::Vector3f(human->ttop_x, human->ttop_y + 0.4, human->ttop_z), cloud);

		cloud_mutex.lock();
		//pcl::Normal normal = normalizer.computeAverageNormal();
		double angle = normalizer.computeHistogramPeekAngle(10);
		cloud_mutex.unlock();

		pose.orientation = tf::createQuaternionMsgFromYaw(/*getROSYaw(normal)*/angle );
		poses.poses.push_back(pose);
	}

	new_cloud = false;
	posesPublisher_ptr->publish(poses);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "region_normalizer");
	ros::NodeHandle nodeHandle;

	ros::Publisher posesPublisher = nodeHandle.advertise<geometry_msgs::PoseArray>("region_normalizer/human_poses", 20);
	posesPublisher_ptr = &posesPublisher;
	ros::Subscriber cloudSubscriber = nodeHandle.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1, cloud_cb_);
	ros::Subscriber humanSubscriber = nodeHandle.subscribe<ground_based_detector::humanArray>("upperbody_filter/detected_human_clusters", 1, human_cb_);

	ros::spin();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2013-2020.
// Modifications copyright (c) 2013-2020 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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)

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_POINT_IN_POLY_WINDING_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_POINT_IN_POLY_WINDING_HPP

#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategy/cartesian/expand_point.hpp>

#include <boost/geometry/strategies/cartesian/point_in_box.hpp>
#include <boost/geometry/strategies/cartesian/disjoint_box_box.hpp>
#include <boost/geometry/strategies/cartesian/side_by_triangle.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/within.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace within
{

/*!
\brief Within detection using winding rule in cartesian coordinate system.
\ingroup strategies
\tparam Point_ \tparam_point
\tparam PointOfSegment_ \tparam_segment_point
\tparam CalculationType \tparam_calculation
\author Barend Gehrels

\qbk{
[heading See also]
[link geometry.reference.algorithms.within.within_3_with_strategy within (with strategy)]
}
 */
template
<
    typename Point_ = void, // for backward compatibility
    typename PointOfSegment_ = Point_, // for backward compatibility
    typename CalculationType = void
>
class cartesian_winding
{
    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : select_calculation_type
            <
                Point,
                PointOfSegment,
                CalculationType
            >
    {};

    /*! subclass to keep state */
    class counter
    {
        int m_count;
        bool m_touches;

        inline int code() const
        {
            return m_touches ? 0 : m_count == 0 ? -1 : 1;
        }

    public :
        friend class cartesian_winding;

        inline counter()
            : m_count(0)
            , m_touches(false)
        {}

    };

public:
    typedef cartesian_tag cs_tag;

    typedef side::side_by_triangle<CalculationType> side_strategy_type;

    static inline side_strategy_type get_side_strategy()
    {
        return side_strategy_type();
    }

    typedef expand::cartesian_point expand_point_strategy_type;

    typedef typename side_strategy_type::envelope_strategy_type envelope_strategy_type;

    static inline envelope_strategy_type get_envelope_strategy()
    {
        return side_strategy_type::get_envelope_strategy();
    }

    typedef typename side_strategy_type::disjoint_strategy_type disjoint_strategy_type;

    static inline disjoint_strategy_type get_disjoint_strategy()
    {
        return side_strategy_type::get_disjoint_strategy();
    }

    typedef typename side_strategy_type::equals_point_point_strategy_type equals_point_point_strategy_type;
    static inline equals_point_point_strategy_type get_equals_point_point_strategy()
    {
        return side_strategy_type::get_equals_point_point_strategy();
    }

    typedef disjoint::cartesian_box_box disjoint_box_box_strategy_type;
    static inline disjoint_box_box_strategy_type get_disjoint_box_box_strategy()
    {
        return disjoint_box_box_strategy_type();
    }

    typedef covered_by::cartesian_point_box disjoint_point_box_strategy_type;

    // Typedefs and static methods to fulfill the concept
    typedef counter state_type;

    template <typename Point, typename PointOfSegment>
    static inline bool apply(Point const& point,
                             PointOfSegment const& s1, PointOfSegment const& s2,
                             counter& state)
    {
        bool eq1 = false;
        bool eq2 = false;

        int count = check_segment(point, s1, s2, state, eq1, eq2);
        if (count != 0)
        {
            int side = 0;
            if (count == 1 || count == -1)
            {
                side = side_equal(point, eq1 ? s1 : s2, count);
            }
            else // count == 2 || count == -2
            {
                // 1 left, -1 right
                side = side_strategy_type::apply(s1, s2, point);
            }

            if (side == 0)
            {
                // Point is lying on segment
                state.m_touches = true;
                state.m_count = 0;
                return false;
            }

            // Side is NEG for right, POS for left.
            // The count is -2 for left, 2 for right (or -1/1)
            // Side positive thus means RIGHT and LEFTSIDE or LEFT and RIGHTSIDE
            // See accompagnying figure (TODO)
            if (side * count > 0)
            {
                state.m_count += count;
            }
        }
        return ! state.m_touches;
    }

    static inline int result(counter const& state)
    {
        return state.code();
    }

private:
    template <typename Point, typename PointOfSegment>
    static inline int check_segment(Point const& point,
                                    PointOfSegment const& seg1,
                                    PointOfSegment const& seg2,
                                    counter& state,
                                    bool& eq1, bool& eq2)
    {
        if (check_touch(point, seg1, seg2, state, eq1, eq2))
        {
            return 0;
        }

        return calculate_count(point, seg1, seg2, eq1, eq2);
    }

    template <typename Point, typename PointOfSegment>
    static inline bool check_touch(Point const& point,
                                   PointOfSegment const& seg1,
                                   PointOfSegment const& seg2,
                                   counter& state,
                                   bool& eq1, bool& eq2)
    {
        typedef typename calculation_type<Point, PointOfSegment>::type calc_t;

        calc_t const px = get<0>(point);
        calc_t const s1x = get<0>(seg1);
        calc_t const s2x = get<0>(seg2);

        eq1 = math::equals(s1x, px);
        eq2 = math::equals(s2x, px);

        // Both equal p -> segment vertical
        // The only thing which has to be done is check if point is ON segment
        if (eq1 && eq2)
        {
            calc_t const py = get<1>(point);
            calc_t const s1y = get<1>(seg1);
            calc_t const s2y = get<1>(seg2);
            if ((s1y <= py && s2y >= py) || (s2y <= py && s1y >= py))
            {
                state.m_touches = true;
            }
            return true;
        }
        return false;
    }

    template <typename Point, typename PointOfSegment>
    static inline int calculate_count(Point const& point,
                                      PointOfSegment const& seg1,
                                      PointOfSegment const& seg2,
                                      bool eq1, bool eq2)
    {
        typedef typename calculation_type<Point, PointOfSegment>::type calc_t;

        calc_t const p = get<0>(point);
        calc_t const s1 = get<0>(seg1);
        calc_t const s2 = get<0>(seg2);

        return eq1 ? (s2 > p ?  1 : -1)  // Point on level s1, E/W depending on s2
             : eq2 ? (s1 > p ? -1 :  1)  // idem
             : s1 < p && s2 > p ?  2     // Point between s1 -> s2 --> E
             : s2 < p && s1 > p ? -2     // Point between s2 -> s1 --> W
             : 0;
    }

    template <typename Point, typename PointOfSegment>
    static inline int side_equal(Point const& point,
                                 PointOfSegment const& se,
                                 int count)
    {
        // NOTE: for D=0 the signs would be reversed
        return math::equals(get<1>(point), get<1>(se)) ?
                0 :
                get<1>(point) < get<1>(se) ?
                    // assuming count is equal to 1 or -1
                    -count : // ( count > 0 ? -1 : 1) :
                    count;   // ( count > 0 ? 1 : -1) ;
    }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename PointLike, typename Geometry, typename AnyTag1, typename AnyTag2>
struct default_strategy<PointLike, Geometry, AnyTag1, AnyTag2, pointlike_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef cartesian_winding<> type;
};

template <typename PointLike, typename Geometry, typename AnyTag1, typename AnyTag2>
struct default_strategy<PointLike, Geometry, AnyTag1, AnyTag2, pointlike_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef cartesian_winding<> type;
};

} // namespace services

#endif

}} // namespace strategy::within

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace strategy { namespace covered_by { namespace services
{

template <typename PointLike, typename Geometry, typename AnyTag1, typename AnyTag2>
struct default_strategy<PointLike, Geometry, AnyTag1, AnyTag2, pointlike_tag, polygonal_tag, cartesian_tag, cartesian_tag>
{
    typedef within::cartesian_winding<> type;
};

template <typename PointLike, typename Geometry, typename AnyTag1, typename AnyTag2>
struct default_strategy<PointLike, Geometry, AnyTag1, AnyTag2, pointlike_tag, linear_tag, cartesian_tag, cartesian_tag>
{
    typedef within::cartesian_winding<> type;
};

}}} // namespace strategy::covered_by::services
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_POINT_IN_POLY_WINDING_HPP


#ifndef DART_NEURAL_UTILS_HPP_
#define DART_NEURAL_UTILS_HPP_

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include "dart/dynamics/Skeleton.hpp"
#include "dart/simulation/World.hpp"

namespace dart {
namespace constraint {
class ConstrainedGroup;
}
namespace simulation {
class World;
}

namespace neural {

struct LossGradient
{
  Eigen::VectorXs lossWrtPosition;
  Eigen::VectorXs lossWrtVelocity;
  Eigen::VectorXs lossWrtTorque;
  Eigen::VectorXs lossWrtMass;
};

struct LossGradientHighLevelAPI
{
  Eigen::VectorXs lossWrtState;
  Eigen::VectorXs lossWrtAction;
  Eigen::VectorXs lossWrtMass;
};

// We don't issue a full import here, because we want this file to be safe to
// import from anywhere else in DART
class ConstrainedGroupGradientMatrices;
class BackpropSnapshot;
class Mapping;
class MappedBackpropSnapshot;

std::shared_ptr<ConstrainedGroupGradientMatrices> createGradientMatrices(
    constraint::ConstrainedGroup& group, s_t timeStep);

/// Takes a step in the world, and returns a backprop snapshot which can be used
/// to backpropagate gradients and compute Jacobians
std::shared_ptr<BackpropSnapshot> forwardPass(
    std::shared_ptr<simulation::World> world, bool idempotent = false);

/// Takes a step in the world, and returns a mapped snapshot which can be used
/// to backpropagate gradients and compute Jacobians in the mapped space
std::shared_ptr<MappedBackpropSnapshot> mappedForwardPass(
    std::shared_ptr<simulation::World> world,
    std::unordered_map<std::string, std::shared_ptr<Mapping>> mappings,
    bool idempotent = false);

struct KnotJacobian
{
  Eigen::MatrixXs knotPosEndPos;
  Eigen::MatrixXs knotVelEndPos;
  Eigen::MatrixXs knotPosEndVel;
  Eigen::MatrixXs knotVelEndVel;
  std::vector<Eigen::MatrixXs> torquesEndPos;
  std::vector<Eigen::MatrixXs> torquesEndVel;
};

//////////////////////////////////////////////////////////////////////////////
// Geometry helpers
//////////////////////////////////////////////////////////////////////////////

enum ConvertToSpace
{
  POS_SPATIAL,
  POS_LINEAR,
  VEL_SPATIAL,
  VEL_LINEAR,
  COM_POS,
  COM_VEL_SPATIAL,
  COM_VEL_LINEAR
};

/// Convert a set of joint positions to a vector of body positions in world
/// space (expressed in log space).
Eigen::MatrixXs convertJointSpaceToWorldSpace(
    const std::shared_ptr<simulation::World>& world,
    const Eigen::MatrixXs& in, /* These can be velocities or positions,
                                    depending on the value of `space` */
    const std::vector<dynamics::BodyNode*>& nodes,
    ConvertToSpace space,
    bool backprop = false,
    bool useIK = true /* Only relevant for backprop */);

//////////////////////////////////////////////
// Similar to above, but just for Skeletons //
//////////////////////////////////////////////

/// Computes a Jacobian that transforms changes in joint angle to changes in
/// body positions (expressed in log space).
Eigen::MatrixXs jointPosToWorldSpatialJacobian(
    const std::shared_ptr<dynamics::Skeleton>& skel,
    const std::vector<dynamics::BodyNode*>& nodes);

/// Computes a Jacobian that transforms changes in joint angle to changes in
/// body positions (expressed in linear space).
Eigen::MatrixXs jointPosToWorldLinearJacobian(
    const std::shared_ptr<dynamics::Skeleton>& skel,
    const std::vector<dynamics::BodyNode*>& nodes);

/// Computes a Jacobian that transforms changes in joint velocity to changes in
/// body velocity (expressed in log space).
Eigen::MatrixXs jointVelToWorldSpatialJacobian(
    const std::shared_ptr<dynamics::Skeleton>& skel,
    const std::vector<dynamics::BodyNode*>& nodes);

/// Computes a Jacobian that transforms changes in joint velocity to changes in
/// body velocity (expressed in linear space).
Eigen::MatrixXs jointVelToWorldLinearJacobian(
    const std::shared_ptr<dynamics::Skeleton>& skel,
    const std::vector<dynamics::BodyNode*>& nodes);

/// Convert a set of joint positions to a vector of body positions in world
/// space (expressed in log space).
Eigen::VectorXs skelConvertJointSpaceToWorldSpace(
    const std::shared_ptr<dynamics::Skeleton>& skel,
    const Eigen::VectorXs& jointValues, /* These can be velocities or positions,
                                    depending on the value of `space` */
    const std::vector<dynamics::BodyNode*>& nodes,
    ConvertToSpace space);

/// Turns losses in terms of body space into losses in terms of joint space
Eigen::VectorXs skelBackpropWorldSpaceToJointSpace(
    const std::shared_ptr<dynamics::Skeleton>& skel,
    const Eigen::VectorXs& bodySpace, /* This is the gradient in body space */
    const std::vector<dynamics::BodyNode*>& nodes,
    ConvertToSpace space, /* This is the source space for our gradient */
    bool useIK = true);

} // namespace neural
} // namespace dart

#endif


/**
 * Copyright (c) 2015
 * Jakob van Santen <jvansanten@icecube.wisc.edu>
 * and the IceCube Collaboration <http://www.icecube.wisc.edu>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
 * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
 * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 *
 * $Id: I3CLSimStepToTableConverter.cxx 177840 2019-12-11 18:50:26Z olivas $
 *
 * @file I3CLSimStepToTableConverter.cxx
 * @version $LastChangedRevision: 177840 $
 * @date $Date: 2019-12-11 11:50:26 -0700 (Wed, 11 Dec 2019) $
 * @author Jakob van Santen
 */

#include "clsim/tabulator/I3CLSimStepToTableConverter.h"
#include "clsim/tabulator/Axes.h"

#include "clsim/function/I3CLSimFunctionConstant.h"

#include "opencl/I3CLSimHelperMath.h"
#include "opencl/I3CLSimHelperGenerateMediumPropertiesSource.h"
#include "opencl/I3CLSimHelperLoadProgramSource.h"
#include "opencl/mwcrng_init.h"
#include "clsim/I3CLSimModuleHelper.h"
#include "clsim/I3CLSimLightSourceToStepConverterUtils.h"
#include "clsim/I3CLSimHelperToFloatString.h"
#include "clsim/cl.hpp"

#include <fitsio.h>
#include <fitsio2.h>

#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>

namespace  {

std::string
loadKernel(const std::string& name, bool header)
{
    const std::string I3_BUILD(getenv("I3_BUILD"));
    const std::string kernelBaseDir = I3_BUILD+"/clsim/resources/kernels/";
    const std::string ext = header ? ".h.cl" : ".c.cl";
    return I3CLSimHelper::LoadProgramSource(kernelBaseDir+name+ext);
}

struct I3CLSimTableEntry {
	uint32_t index;
	float weight;
} __attribute__ ((packed));

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Waddress-of-packed-member"
#endif
struct I3CLSimReferenceParticle {
	I3CLSimReferenceParticle(const I3Particle &source) {
		((cl_float *)(&posAndTime))[0] = source.GetPos().GetX();
		((cl_float *)(&posAndTime))[1] = source.GetPos().GetY();
		((cl_float *)(&posAndTime))[2] = source.GetPos().GetZ();
		((cl_float *)(&posAndTime))[3] = source.GetTime();

		((cl_float *)(&dir))[0] = source.GetDir().GetX();
		((cl_float *)(&dir))[1] = source.GetDir().GetY();
		((cl_float *)(&dir))[2] = source.GetDir().GetZ();
		((cl_float *)(&dir))[3] = 0.;

		const I3Direction &dir = source.GetDir();
		double perpz = hypot(dir.GetX(), dir.GetY());
		I3Direction perpdir = (perpz > 0.) ?
		    I3Direction(-dir.GetX()*dir.GetZ()/perpz, -dir.GetY()*dir.GetZ()/perpz, perpz)
		    : I3Direction(1., 0., 0.);
		((cl_float *)(&perpDir))[0] = perpdir.GetX();
		((cl_float *)(&perpDir))[1] = perpdir.GetY();
		((cl_float *)(&perpDir))[2] = perpdir.GetZ();
		((cl_float *)(&perpDir))[3] = 0.;

	}
	cl_float4 posAndTime;   // x,y,z,time
	cl_float4 dir;          // dx,dy,dz,0
	cl_float4 perpDir;
} __attribute__ ((packed));
#ifdef __clang__
#pragma clang diagnostic pop
#endif

// Brute-force search for the minimum refractive index
std::pair<double, double>
GetMinimumRefractiveIndex(const I3CLSimMediumProperties &med)
{
	std::pair<double, double> n_min(
	    std::numeric_limits<double>::infinity(),
	    std::numeric_limits<double>::infinity());

	for (unsigned j = 0; j < med.GetLayersNum(); j++) {
		if (!med.GetGroupRefractiveIndexOverride(j))
			log_fatal("Medium properties don't know how to calculate group refractive indices");
		const I3CLSimFunction &groupIndex = *(med.GetGroupRefractiveIndexOverride(j));
		const I3CLSimFunction &phaseIndex = *(med.GetPhaseRefractiveIndex(j));

		double wmin = std::max(med.GetMinWavelength(), groupIndex.GetMinWlen());
		double wmax = std::min(med.GetMaxWavelength(), groupIndex.GetMaxWlen());
		for (unsigned i = 0; i < 1000; i++) {
			double n = groupIndex.GetValue(wmin + i*(wmax-wmin));
			if (n > 1 && n < n_min.first) {
				n_min = std::make_pair(n, phaseIndex.GetValue(wmin + i*(wmax-wmin)));
			}
		}
	}

	return n_min;
}

}

I3CLSimStepToTableConverter::I3CLSimStepToTableConverter(I3CLSimOpenCLDevice device,
    clsim::tabulator::AxesConstPtr axes, size_t entriesPerStream, bool storeSquaredWeights,
    I3CLSimMediumPropertiesConstPtr mediumProperties, I3CLSimSpectrumTableConstPtr spectrumTable,
    double referenceArea,
    I3CLSimFunctionConstPtr wavelengthAcceptance, I3CLSimFunctionConstPtr angularAcceptance,
    I3RandomServicePtr rng) : entriesPerStream_(entriesPerStream), stepQueue_(1), run_(true),
    domArea_(referenceArea), stepLength_(1.), axes_(axes),
    numPhotons_(0), sumOfPhotonWeights_(0.)
{
	std::vector<I3CLSimRandomValueConstPtr> wavelengthGenerators;

	wavelengthGenerators.push_back(I3CLSimModuleHelper::makeCherenkovWavelengthGenerator
	                                (wavelengthAcceptance,
	                                 false /*generateCherenkovPhotonsWithoutDispersion_*/,
	                                 mediumProperties
	                                )
	                               );
	// Photonics tables are normalized "per photon", but those photons were
	// drawn from a Cherenkov spectrum between 300 and 600 nm and then
	// weighted by the DOM quantum efficiency (QE) at recording time. Since
	// we draw photon wavelengths directly from a QE-weighted Cherenkov
	// spectrum, each of our photons represents on average more than one
	// Photonics photon.
	{
		using I3CLSimLightSourceToStepConverterUtils::NumberOfPhotonsPerMeter;

		I3CLSimFunctionConstant uno(1.);
		I3CLSimFunctionConstPtr nPhase = mediumProperties->GetPhaseRefractiveIndex(0);
		spectralBiasFactor_ = NumberOfPhotonsPerMeter(*nPhase, uno, 300*I3Units::nanometer, 600*I3Units::nanometer)
		    / NumberOfPhotonsPerMeter(*nPhase, *wavelengthAcceptance, mediumProperties->GetMinWavelength(), mediumProperties->GetMaxWavelength());
		log_info_stream("Each photon is worth "<<spectralBiasFactor_<<" Photonics photons");
	}
	if ((spectrumTable) && (spectrumTable->size() > 1)) {
	    // a spectrum table has been configured and it contains more than the
	    // default Cherenkov spectrum at index #0.

	    for (std::size_t i=1;i<spectrumTable->size();++i)
	    {
	        wavelengthGenerators.push_back(I3CLSimModuleHelper::makeWavelengthGenerator
	                                        ((*spectrumTable)[i],
	                                         wavelengthAcceptance,
	                                         mediumProperties
	                                         )
	                                        );
	    }

	    log_debug("%zu additional (non-Cherenkov) wavelength generators (spectra) have been configured.",
	             spectrumTable->size()-1);
	}

	std::vector<std::string> sources;

	const std::string I3_BUILD(getenv("I3_BUILD"));
	const std::string kernelBaseDir = I3_BUILD+"/clsim/resources/kernels/";

	std::ostringstream preamble;
	preamble <<                                                               \
	    "#define SAVE_ALL_PHOTONS\n"                                          \
	    "#define SAVE_ALL_PHOTONS_PRESCALE 1\n"                               \
	    "#define PROPAGATE_FOR_FIXED_NUMBER_OF_ABSORPTION_LENGTHS 42\n"       \
	    "#define TABULATE\n"                                                  \
	    "//#define DOM_RADIUS "<<I3CLSimHelper::ToFloatString(0.16510*I3Units::m)<<"\n"\
	    "//#define PRINTF_ENABLED\n"                                            \
	;
	if (axes_->GetNDim() > 4)
		preamble << "#define TABULATE_IMPACT_ANGLE\n";
	preamble << "#define TABLE_ENTRIES_PER_STREAM " << entriesPerStream_ << "\n";
	preamble << "#define VOLUME_MODE_STEP "<<I3CLSimHelper::ToFloatString(stepLength_)<<"\n";
	minimumRefractiveIndex_ = GetMinimumRefractiveIndex(*mediumProperties);

	preamble << "__constant floating_t min_invGroupVel = " << I3CLSimHelper::ToFloatString(
	    minimumRefractiveIndex_.first/I3Constants::c) << ";\n";
	preamble << "__constant floating_t tan_thetaC = " << I3CLSimHelper::ToFloatString(
	    std::sqrt(minimumRefractiveIndex_.second*minimumRefractiveIndex_.second-1.)) << ";\n";

	sources.push_back(I3CLSimHelper::GetMathPreamble(device, false /* single precision for now */));
	sources.push_back(preamble.str());
	sources.push_back(I3CLSimHelper::LoadProgramSource(kernelBaseDir+"/mwcrng_kernel.cl"));
	sources.push_back(I3CLSimHelper::GenerateWavelengthGeneratorSource(wavelengthGenerators));
	sources.push_back(wavelengthAcceptance->GetOpenCLFunction("getWavelengthBias"));
	sources.push_back(I3CLSimHelper::GenerateMediumPropertiesSource(*mediumProperties));
	sources.push_back(angularAcceptance->GetOpenCLFunction("getAngularAcceptance"));

	sources.push_back(loadKernel("propagation_kernel", true));
	sources.push_back(axes_->GenerateBinningCode());
	sources.push_back(loadKernel("propagation_kernel", false));

	binContent_.resize(axes_->GetNBins(), 0);
	if (storeSquaredWeights)
		squaredWeights_.resize(axes->GetNBins(), 0);

#ifndef NDEBUG
	std::stringstream source;
	BOOST_FOREACH(std::string &part, sources)
		source << part;
	std::string line;
	unsigned lineno = 1;
	while (std::getline(source, line)) {
		std::cout << std::setw(4) << lineno << " " << line << std::endl;
		lineno++;
	}
#endif

	{
		VECTOR_CLASS<cl::Device> devices(1, *(device.GetDeviceHandle()));
		cl_context_properties properties[] =
		{ CL_CONTEXT_PLATFORM, (cl_context_properties)(*(device.GetPlatformHandle()))(), 0};

		context_ = cl::Context(devices, properties);

		std::string BuildOptions;
		BuildOptions += "-cl-mad-enable";

		cl::Program::Sources source;
		BOOST_FOREACH(const std::string &part, sources)
			source.push_back(std::make_pair(part.c_str(), part.size()));

		cl::Device device = devices[0];
		cl::Program program(context_, source);
		try {
			program.build(devices, BuildOptions.c_str());
		} catch (cl::Error &err) {
	        std::string deviceName = device.getInfo<CL_DEVICE_NAME>();
	        log_error("  * build status on %s\"", deviceName.c_str());
	        log_error("==============================");
	        log_error("Build Status: %s", boost::lexical_cast<std::string>(program.getBuildInfo<CL_PROGRAM_BUILD_STATUS>(device)).c_str());
	        log_error("Build Options: %s", boost::lexical_cast<std::string>(program.getBuildInfo<CL_PROGRAM_BUILD_OPTIONS>(device)).c_str());
	        log_error("Build Log: %s", boost::lexical_cast<std::string>(program.getBuildInfo<CL_PROGRAM_BUILD_LOG>(device)).c_str());
	        log_error("==============================");
			log_fatal_stream(err.what() << ": " << err.errstr());
		}

		commandQueue_ = cl::CommandQueue(context_, device, CL_QUEUE_PROFILING_ENABLE);

		cl::Kernel kernel(program, "propKernel");

		maxNumWorkitems_ = std::min(kernel.getWorkGroupInfo<CL_KERNEL_WORK_GROUP_SIZE>(device), size_t(1));
		log_debug_stream("max work group size " << maxNumWorkitems_);
		log_debug_stream(device.getInfo<CL_DEVICE_NAME>() << " max memory "<<device.getInfo<CL_DEVICE_MAX_MEM_ALLOC_SIZE>());

		harvesterThread_ = boost::thread(boost::bind(&I3CLSimStepToTableConverter::FetchSteps, this, kernel, rng));
	}

}

I3CLSimStepToTableConverter::~I3CLSimStepToTableConverter()
{
	Finish();
}

void
I3CLSimStepToTableConverter::EnqueueSteps(I3CLSimStepSeriesConstPtr steps, I3ParticleConstPtr reference)
{
	if (!steps)
		return;
	BOOST_FOREACH(const I3CLSimStep &step, *steps) {
		numPhotons_ += step.GetNumPhotons();
		sumOfPhotonWeights_ += step.GetNumPhotons()*step.GetWeight();
	}

	stepQueue_.Put(bunch_t(steps, reference));
}

void
I3CLSimStepToTableConverter::Finish()
{
	if (harvesterThread_.joinable()) {
		run_ = false;
		log_debug("Finish");
		harvesterThread_.join();
	}
}

namespace {

struct DeviceBuffers {
	DeviceBuffers() {};
	DeviceBuffers(cl::Context, I3RandomServicePtr, size_t streams,
	    size_t entriesPerStream);
	struct {
		cl::Buffer x, a;
	} mwc;
	cl::Buffer inputSteps;
	cl::Buffer referenceSource;
	cl::Buffer outputEntries;
	cl::Buffer numEntries;
};

DeviceBuffers::DeviceBuffers(cl::Context context,
    I3RandomServicePtr rng, size_t streams, size_t entriesPerStream)
{
	std::vector<uint64_t> xv(streams);
	std::vector<uint32_t> av(streams);

	init_MWC_RNG(&xv[0], &av[0], streams, rng);

	mwc.x = cl::Buffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
	    streams*sizeof(uint64_t), &xv[0]);
	mwc.a = cl::Buffer(context, CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
	    streams*sizeof(uint32_t), &av[0]);

	inputSteps = cl::Buffer(context, CL_MEM_READ_WRITE | CL_MEM_ALLOC_HOST_PTR,
	    streams*sizeof(I3CLSimStep));
	referenceSource = cl::Buffer(context, CL_MEM_READ_ONLY | CL_MEM_ALLOC_HOST_PTR,
	    sizeof(I3CLSimReferenceParticle));
	try {
	outputEntries = cl::Buffer(context, CL_MEM_WRITE_ONLY | CL_MEM_ALLOC_HOST_PTR,
	    streams*entriesPerStream*sizeof(I3CLSimTableEntry));
	} catch (cl::Error &err) {
		log_error_stream(err.what() << " " << err.errstr());
		throw;
	}
	numEntries = cl::Buffer(context, CL_MEM_WRITE_ONLY | CL_MEM_ALLOC_HOST_PTR,
	    streams*sizeof(uint32_t));

}

struct KernelStatistics {

	KernelStatistics() : last_timestamp_(boost::posix_time::microsec_clock::universal_time()),
	    photons_(0), steps_(0), misses_(0), hostTime_(0), deviceTime_(0)
	{}
	~KernelStatistics()
	{
		double util = 100.*double(deviceTime_)/hostTime_;
		log_info_stream("Tracked "<<photons_<<" photons in "<<(double(hostTime_)*1e-9)<<"s: "<<1e-6*double(hostTime_)/photons_ << " millisec/photon ("<<util<<"% in kernel)");
		if (misses_*100 > steps_)
			log_warn_stream(misses_<<" of "<<steps_
			    <<" photon bunches ("<<std::setprecision(2)<<((100.*misses_)/steps_)
			    <<"%) ran out of storage space. "
			    "You might want to increase the storage space per photon bunch.");
	}

	void Record(const cl::Event &kernelFinished, uint64_t n_photons, size_t steps, size_t misses)
	{
		boost::posix_time::ptime this_timestamp(boost::posix_time::microsec_clock::universal_time());

		uint64_t timeStart, timeEnd;
		kernelFinished.getProfilingInfo(CL_PROFILING_COMMAND_START, &timeStart);
		kernelFinished.getProfilingInfo(CL_PROFILING_COMMAND_END, &timeEnd);

		photons_ += n_photons;
		deviceTime_ += (timeEnd-timeStart);
		hostTime_ += (this_timestamp - last_timestamp_).total_nanoseconds();
		misses_ += misses;
		steps_ += steps;

		last_timestamp_ = this_timestamp;
	}

	void Start()
	{
		last_timestamp_ = boost::posix_time::microsec_clock::universal_time();
	}

	boost::posix_time::ptime last_timestamp_;
	size_t photons_, steps_, misses_, hostTime_, deviceTime_;

	SET_LOGGER("I3CLSimStepToTableConverter");
};

}

void
I3CLSimStepToTableConverter::FetchSteps(cl::Kernel kernel, I3RandomServicePtr rng)
{

	DeviceBuffers buffers(context_, rng, maxNumWorkitems_, entriesPerStream_);

	// Set kernel arguments
	uint args = 0;
	kernel.setArg(args++, buffers.inputSteps);
	kernel.setArg(args++, buffers.referenceSource);
	kernel.setArg(args++, buffers.outputEntries);
	kernel.setArg(args++, buffers.numEntries);
	kernel.setArg(args++, buffers.mwc.x);
	kernel.setArg(args++, buffers.mwc.a);

	I3CLSimStepSeries osteps(maxNumWorkitems_);
	std::vector<uint32_t> numEntries(maxNumWorkitems_);
	std::vector<I3CLSimTableEntry> tableEntries(maxNumWorkitems_*entriesPerStream_);

	KernelStatistics stats;

	bunch_t bunch;
	while (1) {

		// Work on either the remainder of the last bunch or the next bunch
		// in the queue
		if (!bunch.first && !stepQueue_.GetNonBlocking(bunch)) {
			if (run_) {
				continue;
			} else {
				return;
			}
		}

		size_t n_photons = 0;
		size_t real_steps = 0;
		BOOST_FOREACH(const I3CLSimStep &step, *bunch.first) {
			if (step.GetNumPhotons() > 0) {
				n_photons += step.GetNumPhotons();
				real_steps++;
			}
		}

		VECTOR_CLASS<cl::Event> buffersFilled(3);
		VECTOR_CLASS<cl::Event> kernelFinished(1);
		VECTOR_CLASS<cl::Event> buffersRead(3);

		const size_t items = bunch.first->size();
		assert(items <= maxNumWorkitems_);
		commandQueue_.enqueueWriteBuffer(buffers.inputSteps, CL_FALSE, 0,
		    items*sizeof(I3CLSimStep), &(*bunch.first)[0], NULL, &buffersFilled[0]);

		I3CLSimReferenceParticle ref(*bunch.second);
		commandQueue_.enqueueWriteBuffer(buffers.referenceSource, CL_FALSE, 0,
		    sizeof(I3CLSimReferenceParticle), &ref, NULL, &buffersFilled[1]);

		commandQueue_.enqueueFillBuffer<uint32_t>(buffers.numEntries, 0u /*pattern*/,
		    0 /*offset*/, items*sizeof(uint32_t) /*size*/, NULL, &buffersFilled[2]);
		commandQueue_.flush();

		try {
		commandQueue_.enqueueNDRangeKernel(kernel, cl::NullRange,
		    cl::NDRange(items), cl::NDRange(maxNumWorkitems_),
		    &buffersFilled, &kernelFinished[0]);
		} catch (cl::Error &err) {
			log_error_stream(err.what() << " " << err.errstr());
			throw;
		}

		commandQueue_.enqueueReadBuffer(buffers.inputSteps, CL_FALSE, 0,
		    items*sizeof(I3CLSimStep), &osteps[0], &kernelFinished, &buffersRead[0]);
		commandQueue_.enqueueReadBuffer(buffers.numEntries, CL_FALSE, 0,
		    items*sizeof(uint32_t), &numEntries[0], &kernelFinished, &buffersRead[1]);
		commandQueue_.enqueueReadBuffer(buffers.outputEntries, CL_FALSE, 0,
		    items*entriesPerStream_*sizeof(I3CLSimTableEntry), &tableEntries[0], &kernelFinished, &buffersRead[2]);

		commandQueue_.flush();

		cl::Event::waitForEvents(buffersRead);

		I3CLSimStepSeriesPtr rsteps;
		size_t misses = 0;
		for (size_t i = 0; i < items; i++) {
			// If any steps ran out of space, return them to the work list
			if (osteps[i].GetNumPhotons() > 0) {
				log_trace_stream(osteps[i].GetNumPhotons() << " left");
				if (!rsteps)
					rsteps = I3CLSimStepSeriesPtr(new I3CLSimStepSeries);
				rsteps->push_back(osteps[i]);
				n_photons -= osteps[i].GetNumPhotons();
				misses++;
			}
		}

		if (rsteps && rsteps->size() > 0) {
			assert(rsteps->size() <= maxNumWorkitems_);
			// Pad out to workgroup size
			I3CLSimStep dummy = rsteps->front();
			dummy.SetNumPhotons(0);
			while (rsteps->size() < maxNumWorkitems_)
				rsteps->push_back(dummy);
			bunch.first = rsteps;
		} else {
			// No steps ran out of space; reset the work list.
			bunch.first.reset();
		}

		for (size_t i = 0; i < items; i++) {
			size_t size = numEntries[i];
			size_t offset = i*entriesPerStream_;
			for (size_t j = 0; j < size; j++) {
				binContent_[tableEntries[offset+j].index] += tableEntries[offset+j].weight;
			}
			if (squaredWeights_.size() > 0) {
				for (size_t j = 0; j < size; j++) {
					squaredWeights_[tableEntries[offset+j].index] += std::pow(tableEntries[offset+j].weight, 2);
				}
			}
		}

		stats.Record(kernelFinished[0], n_photons, real_steps, misses);
	} // while (1)
}

void
I3CLSimStepToTableConverter::Normalize()
{
	const unsigned ndim = axes_->GetNDim();
	const std::vector<size_t> shape = axes_->GetShape();
	const std::vector<size_t> strides = axes_->GetStrides();
	std::vector<size_t> idxs(ndim, 0);

	// NB: assume that the first 3 dimensions are spatial
	const size_t spatial_stride = strides[2];
	for (size_t offset = 0; offset < binContent_.size(); offset += spatial_stride) {
		// unravel index
		for (unsigned j=0; j < ndim; j++) {
			// each dimension has an under- and an overflow bin.
			idxs[j] = std::min(std::max(int(offset/strides[j] % shape[j])-1, 0), int(shape[j]-3));
			assert(idxs[j] < shape[j]-2);
		}
		assert(idxs[ndim-1] == 0);

		// apply volume normalization to each spatial cell
		double norm = axes_->GetBinVolume(idxs)/(stepLength_*domArea_);
		for (size_t i=0; i < spatial_stride; i++)
			binContent_[i+offset] /= norm;
		// apply to squared weights as well if needed
		if (squaredWeights_.size() > 0) {
			norm *= norm;
			for (size_t i=0; i < spatial_stride; i++)
				squaredWeights_[i+offset] /= norm;
		}
	}
}

namespace {

std::string error_text(int error)
{
	std::string text(30, '\0');
	if (error != 0) {
		fits_get_errstatus(error, &text[0]);
	}

	return text;
}

/*
 * Create the bin content array with transposed axis
 * counts, like PyFITS does.
 */
void create_image(fitsfile *fits, std::vector<size_t> &shape, std::string name=std::string())
{
	int error(0);
	std::vector<long> naxes(shape.size());
	std::reverse_copy(shape.begin(), shape.end(), naxes.begin());
	fits_create_img(fits, FLOAT_IMG, shape.size(), &naxes[0], &error);
	if (error != 0) {
		log_fatal_stream("Could not create image: " << error_text(error));
	}
	if (name.size() > 0) {
		fits_write_key(fits, TSTRING, "EXTNAME", (void*)(name.c_str()),
		    NULL, &error);
	}
	if (error != 0) {
		log_fatal_stream("Could name HDU "<<name<<": " << error_text(error));
	}
}

void write_pixels(fitsfile *fits, std::vector<size_t> &shape,
    std::vector<float> &pixels)
{
	int error(0);
	std::vector<long> fpixel(shape.size(), 1);
	fits_write_pix(fits, TFLOAT, &fpixel[0], pixels.size(),
	    &pixels[0], &error);
	if (error != 0) {
		log_fatal_stream("Could not fill image: " << error_text(error));
	}
}

}

void I3CLSimStepToTableConverter::WriteFITSFile(const std::string &path, boost::python::dict tableHeader)
{
	fitsfile *fits;
	int error = 0;

	fits_create_diskfile(&fits, path.c_str(), &error);
	if (error != 0) {
		log_fatal_stream("Could not create " << path << ": " << error_text(error));
	}

	/*
	 * Write bin content
	 */
	this->Normalize();
	{
		std::vector<size_t> shape(axes_->GetShape());
		create_image(fits, shape);
		write_pixels(fits, shape, binContent_);
	}

	// Fill in things that only we know
	tableHeader["n_photons"] = spectralBiasFactor_*sumOfPhotonWeights_;
	tableHeader["n_group"] = minimumRefractiveIndex_.first;
	tableHeader["n_phase"] = minimumRefractiveIndex_.second;
	// Write header keywords
	{
		namespace bp = boost::python;

		bp::list keys = tableHeader.keys();
		for (int i = 0; i < bp::len(keys); i++) {
			bp::object key = keys[i];
			bp::object value = tableHeader[key];

			std::ostringstream name;
			name << "hierarch _i3_" << bp::extract<std::string>(key)();

			bp::extract<int> inty(value);
			bp::extract<double> doubly(value);
			if (inty.check()) {
				int v = inty();
				fits_write_key(fits, TINT, name.str().c_str(), &v,
				    NULL, &error);
			} else if (doubly.check()) {
				double v = doubly();
				fits_write_key(fits, TDOUBLE, name.str().c_str(), &v,
				    NULL, &error);
			}
			if (error != 0) {
				log_fatal_stream("Could not write header keyword "<<name.str()<<": " << error_text(error));
			}
		}
	}

	/*
	 * Write squared weights
	 */
	if (squaredWeights_.size() > 0) {
		std::vector<size_t> shape(axes_->GetShape());
		create_image(fits, shape, "ERRORS");
		write_pixels(fits, shape, squaredWeights_);
	}

	/*
	 * Write each of the bin edge vectors in an extension HDU
	 */
	for (unsigned i = 0; i < axes_->GetNDim(); i++) {
		std::ostringstream name;
		name << "EDGES" << i;
		long fpixel = 1;
		std::vector<double> edges = axes_->at(i)->GetBinEdges();
		long size = edges.size();

		fits_create_img(fits, DOUBLE_IMG, 1, &size, &error);
		if (error != 0) {
			log_fatal_stream("Could not create edge array "<<i<<": " << error_text(error));
		}
		fits_write_key(fits, TSTRING, "EXTNAME", (void*)(name.str().c_str()),
		    NULL, &error);
		if (error != 0) {
			log_fatal_stream("Could not name HDU "<<name.str()<<": " << error_text(error));
		}

		fits_write_pix(fits, TDOUBLE, &fpixel, size,
		    &edges[0], &error);
		if (error != 0) {
			log_fatal_stream("Could not write edge array "<<i<<": " << error_text(error));
		}
	}

	fits_close_file(fits, &error);
	if (error != 0) {
		log_fatal_stream("Could not close " << path << ": " << error_text(error));
	}
}


//==============================================================================
//         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 NT2_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_PIO2_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_REM_PIO2_HPP_INCLUDED

#include <nt2/trigonometric/functions/rem_pio2.hpp>
#include <nt2/trigonometric/functions/scalar/impl/trigo/selection_tags.hpp>
#include <nt2/include/functions/simd/rem_pio2_straight.hpp>
#include <nt2/include/functions/simd/rem_pio2_medium.hpp>
#include <nt2/include/functions/simd/rem_pio2_cephes.hpp>
#include <nt2/include/functions/simd/aligned_load.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/simd/preprocessor/aligned_type.hpp>

namespace nt2 { namespace ext
{

  BOOST_DISPATCH_IMPLEMENT  (  rem_pio2_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& a0,A0 & xr,A0 & xc) const
    {
      typedef typename meta::scalar_of<A0>::type          stype;
      typedef typename meta::scalar_of<result_type>::type itype;
      static const nt2::uint32_t size = meta::cardinal_of<result_type>::value;
      BOOST_SIMD_ALIGNED_TYPE(itype) tmp[size];
      BOOST_SIMD_ALIGNED_TYPE(stype) txr[size];
      BOOST_SIMD_ALIGNED_TYPE(stype) txc[size];
      for(nt2::uint32_t i=0; i!=size; ++i)
      {
        // This prevent the may_alias ICE
        stype a = a0[i], r = txr[i], c = txc[i];
        tmp[i] =  nt2::rem_pio2(a, r, c);
        txr[i] = r;
        txc[i] = c;
      }

      xr = aligned_load<A0>(&txr[0], 0);
      xc = aligned_load<A0>(&txc[0], 0);
      return aligned_load<result_type>(&tmp[0], 0);
    }
  };

} }
#endif


//
// Created by june on 19. 12. 5..
//

// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <urdf/model.h>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include <geometry_msgs/WrenchStamped.h>
#include "utils.h"
#include "dualarm_controller/TaskCurrentState.h"
#include "dualarm_controller/TaskDesiredState.h"

//manipulability
#include <ellipsoid.h>
#include <similarity.h>

// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/chainjnttojacsolver.hpp>        // jacobian
#include <kdl/chainfksolverpos_recursive.hpp> // forward kinematics
#include <boost/scoped_ptr.hpp>

#define _USE_MATH_DEFINES
#include <cmath>

#include <SerialManipulator.h>
#include <Controller.h>
#include <Motion.h>

#define D2R M_PI/180.0
#define R2D 180.0/M_PI

#define A 0.12
#define b1 0.55
#define b2 -0.43
#define b3 0.45
#define f 0.2

#define l_p1 0.45
#define l_p2 0.43
#define l_p3 0.39

#define Deg_A 70
#define Deg_f 0.5

namespace  dualarm_controller
{
    class ClosedLoopIK_Control : public controller_interface::Controller<hardware_interface::EffortJointInterface>
    {
    public:
        bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
        {
            // ********* 1. Get joint name / gain from the parameter server *********
            // 1.0 Control objective & Inverse Kinematics mode
            if (!n.getParam("ctr_obj", ctr_obj_))
            {
                ROS_ERROR("Could not find control objective");
                return false;
            }

            if (!n.getParam("ik_mode", ik_mode_))
            {
                ROS_ERROR("Could not find control objective");
                return false;
            }

            if( ctr_obj_ == 7 && ik_mode_ != 4 )
            {
                ROS_ERROR("!! If the ctr_obj is 8, ik_mode_ should be 4 !!");
                return false;
            }

            // 1.1 Joint Name
            if (!n.getParam("joints", joint_names_))
            {
                ROS_ERROR("Could not find joint name");
                return false;
            }
            n_joints_ = joint_names_.size();

            if (n_joints_ == 0)
            {
                ROS_ERROR("List of joint names is empty.");
                return false;
            }
            else
            {
                ROS_INFO("Found %d joint names", n_joints_);
                for (int i = 0; i < n_joints_; i++)
                {
                    ROS_INFO("%s", joint_names_[i].c_str());
                }
            }

            // 1.2 Gain
            // 1.2.1 Joint Controller
            Kp_.data.setZero(n_joints_);
            Kd_.data.setZero(n_joints_);
            Ki_.data.setZero(n_joints_);
            K_inf_.data.setZero(n_joints_);

            for (size_t i = 0; i < n_joints_; i++)
            {
                std::string si = std::to_string(i + 1);
                if (!n.getParam("/dualarm/closedloopik_control/gains/dualarm_joint" + si + "/pid/p", Kp_.data(i)))
                {
                    ROS_ERROR("Cannot find pid/p gain");
                    return false;
                }

                if (!n.getParam("/dualarm/closedloopik_control/gains/dualarm_joint" + si + "/pid/i", Ki_.data(i)))
                {
                    ROS_ERROR("Cannot find pid/i gain");
                    return false;
                }

                if (!n.getParam("/dualarm/closedloopik_control/gains/dualarm_joint" + si + "/pid/d", Kd_.data(i)))
                {
                    ROS_ERROR("Cannot find pid/d gain");
                    return false;
                }

                if (!n.getParam("/dualarm/closedloopik_control/gains/dualarm_joint" + si + "/pid/h", K_inf_.data(i)))
                {
                    ROS_ERROR("Cannot find pid/h gain");
                    return false;
                }
            }

            // 1.2.2 Closed-loop Inverse Kinematics Controller

            if (!n.getParam("/dualarm/closedloopik_control/clik_gain/K_pos", K_trans))
            {
                ROS_ERROR("Cannot find clik translation gain");
                return false;
            }

            if (!n.getParam("/dualarm/closedloopik_control/clik_gain/K_ori", K_rot))
            {
                ROS_ERROR("Cannot find clik rotation gain");
                return false;
            }

            // 2. ********* urdf *********
            urdf::Model urdf;
            if (!urdf.initParam("robot_description"))
            {
                ROS_ERROR("Failed to parse urdf file");
                return false;
            }
            else
            {
                ROS_INFO_STREAM("Found robot_description");
            }

            // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
            for (int i = 0; i < n_joints_; i++)
            {
                try
                {
                    joints_.push_back(hw->getHandle(joint_names_[i]));
                }
                catch (const hardware_interface::HardwareInterfaceException &e)
                {
                    ROS_ERROR_STREAM("Exception thrown: " << e.what());
                    return false;
                }

                urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
                if (!joint_urdf)
                {
                    ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                    return false;
                }
                joint_urdfs_.push_back(joint_urdf);
            }

            // 4. ********* KDL *********
            // 4.1 kdl parser
            if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
            {
                ROS_ERROR("Failed to construct kdl tree");
                return false;
            }
            else
            {
                ROS_INFO_STREAM("Constructed kdl tree");
            }

            // 4.2 kdl chain
            std::string root_name, tip_name1, tip_name2;
            if (!n.getParam("root_link", root_name))
            {
                ROS_ERROR("Could not find root link name");
                return false;
            }
            if (!n.getParam("tip_link1", tip_name1))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }
            if (!n.getParam("tip_link2", tip_name2))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }

            if (!kdl_tree_.getChain(root_name, tip_name1, kdl_chain_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_ERROR_STREAM("  Chain has " << kdl_chain_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Chain has " << kdl_chain_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO_STREAM("Got kdl first chain");
                ROS_INFO_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_INFO_STREAM("  Chain has " << kdl_chain_.getNrOfJoints() << " joints");
                ROS_INFO_STREAM("  Chain has " << kdl_chain_.getNrOfSegments() << " segments");
            }

            if(!kdl_tree_.getChain(root_name, tip_name2, kdl_chain2_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name2);
                ROS_ERROR_STREAM("  Chain has " << kdl_chain2_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Chain has " << kdl_chain2_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO_STREAM("Got kdl second chain");
                ROS_INFO_STREAM("  " << root_name << " --> " << tip_name2);
                ROS_INFO_STREAM("  Chain has " << kdl_chain2_.getNrOfJoints() << " joints");
                ROS_INFO_STREAM("  Chain has " << kdl_chain2_.getNrOfSegments() << " segments");
            }

            // 4.3 inverse dynamics solver 초기화
            g_kdl_ = KDL::Vector::Zero();
            g_kdl_(2) = -9.81; // 0: x-axis 1: y-axis 2: z-axis

            id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, g_kdl_));
            jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
            fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
            J1_kdl_.resize(kdl_chain_.getNrOfJoints());
            M_kdl_.resize(kdl_chain_.getNrOfJoints());
            C_kdl_.resize(kdl_chain_.getNrOfJoints());
            G_kdl_.resize(kdl_chain_.getNrOfJoints());

            id_solver1_.reset(new KDL::ChainDynParam(kdl_chain2_, g_kdl_));
            jnt_to_jac_solver1_.reset(new KDL::ChainJntToJacSolver(kdl_chain2_));
            fk_pos_solver1_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain2_));
            J2_kdl_.resize(kdl_chain2_.getNrOfJoints());
            M1_kdl_.resize(kdl_chain2_.getNrOfJoints());
            C1_kdl_.resize(kdl_chain2_.getNrOfJoints());
            G1_kdl_.resize(kdl_chain2_.getNrOfJoints());

            // ********* 5. 각종 변수 초기화 *********
            // 5.1 KDL Vector 초기화 (사이즈 정의 및 값 0)
            x_cmd_.data = Eigen::VectorXd::Zero(12);
            targetpos.setZero(12);
            ex_.setZero(12);
            ex_dot_.setZero(12);
            dx.setZero(12);
            dxdot.setZero(12);

            qd_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

            q_.data = Eigen::VectorXd::Zero(n_joints_);
            qdot_.data = Eigen::VectorXd::Zero(n_joints_);
            torque.setZero(n_joints_);
            ft_sensor.setZero(12);

            wpInv_lambda[0].setZero(3);
            wpInv_lambda[1].setZero(3);

            // ********* 6. ROS 명령어 *********
            // 6.1 publisher
            state_pub_.reset(new realtime_tools::RealtimePublisher<dualarm_controller::TaskCurrentState>(n, "states", 10));
            state_pub_->msg_.header.stamp = ros::Time::now();
            state_pub_->msg_.header.frame_id = "dualarm";
            state_pub_->msg_.header.seq=0;
            for(int i=0; i<(n_joints_-1); i++)
            {
                state_pub_->msg_.q.push_back(q_.data(i));
                state_pub_->msg_.qdot.push_back(qdot_.data(i));
                state_pub_->msg_.dq.push_back(qd_.data(i));
                state_pub_->msg_.dqdot.push_back(qd_dot_.data(i));
                state_pub_->msg_.torque.push_back(torque(i));

            }
            for(int j=0; j<2; j++) {
                state_pub_->msg_.InverseConditionNum.push_back(InverseConditionNumber[j]);
                state_pub_->msg_.SingleMM.push_back(SingleMM[j]);
            }
            state_pub_->msg_.x.resize(2);
            state_pub_->msg_.dx.resize(2);
            state_pub_->msg_.MM = MM;
            state_pub_->msg_.DAMM = DAMM;
            state_pub_->msg_.TODAMM = TODAMM2;
            state_pub_->msg_.lambda1.resize(3);
            state_pub_->msg_.lambda2.resize(3);
            state_pub_->msg_.Kp_R = K_rot;
            state_pub_->msg_.Kp_T = K_trans;

            pub_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));

            // 6.2 subsriber
            const auto joint_state_cb = utils::makeCallback<dualarm_controller::TaskDesiredState>([&](const auto& msg){
                ControlMode = msg.Index1;
                ControlSubMode = msg.Index2;
                ControlMotion = msg.SubIndex;
                targetpos(0) = msg.dx[0].orientation.x*DEGtoRAD;
                targetpos(1) = msg.dx[0].orientation.y*DEGtoRAD;
                targetpos(2) = msg.dx[0].orientation.z*DEGtoRAD;
                targetpos(3) = msg.dx[0].position.x;
                targetpos(4) = msg.dx[0].position.y;
                targetpos(5) = msg.dx[0].position.z;
                targetpos(6) = msg.dx[1].orientation.x*DEGtoRAD;
                targetpos(7) = msg.dx[1].orientation.y*DEGtoRAD;
                targetpos(8) = msg.dx[1].orientation.z*DEGtoRAD;
                targetpos(9) = msg.dx[1].position.x;
                targetpos(10) = msg.dx[1].position.y;
                targetpos(11) = msg.dx[1].position.z;

            });
            sub_x_cmd_ = n.subscribe<dualarm_controller::TaskDesiredState>( "command", 5, joint_state_cb);
            sub_ft_sensor_R = n.subscribe<geometry_msgs::WrenchStamped>("/ft_sensor_topic_R", 5, &ClosedLoopIK_Control::UpdateFTsensorR, this);
            sub_ft_sensor_L = n.subscribe<geometry_msgs::WrenchStamped>("/ft_sensor_topic_L", 5, &ClosedLoopIK_Control::UpdateFTsensorL, this);

            return true;
        }

        void UpdateFTsensorR(const geometry_msgs::WrenchStamped::ConstPtr &msg)
        {
            geometry_msgs::Wrench ft_measure = msg->wrench;
            ft_sensor(0) = ft_measure.torque.x;
            ft_sensor(1) = ft_measure.torque.y;
            ft_sensor(2) = ft_measure.torque.z;
            ft_sensor(3) = ft_measure.force.x;
            ft_sensor(4) = ft_measure.force.y;
            ft_sensor(5) = ft_measure.force.z;
        }

        void UpdateFTsensorL(const geometry_msgs::WrenchStamped::ConstPtr &msg)
        {
            geometry_msgs::Wrench ft_measure = msg->wrench;
            ft_sensor(6) = ft_measure.torque.x;
            ft_sensor(7) = ft_measure.torque.y;
            ft_sensor(8) = ft_measure.torque.z;
            ft_sensor(9) = ft_measure.force.x;
            ft_sensor(10) = ft_measure.force.y;
            ft_sensor(11) = ft_measure.force.z;
        }
        void starting(const ros::Time &time) override
        {
            t = 0.0;
            InitTime=1.0;

            ROS_INFO("Starting Closed-loop Inverse Dynamics Controller");

            cManipulator = std::make_shared<SerialManipulator>();

            Control = std::make_unique<HYUControl::Controller>(cManipulator);
            motion = std::make_unique<HYUControl::Motion>(cManipulator);

            cManipulator->UpdateManipulatorParam();

            CLIK_GAIN_DUMY.setZero(12);
            CLIK_GAIN.setZero(12);
            CLIK_GAIN(0) = K_rot;
            CLIK_GAIN(1) = K_rot;
            CLIK_GAIN(2) = K_rot;
            CLIK_GAIN(3) = K_trans;
            CLIK_GAIN(4) = K_trans;
            CLIK_GAIN(5) = K_trans;
            CLIK_GAIN(6) = K_rot;
            CLIK_GAIN(7) = K_rot;
            CLIK_GAIN(8) = K_rot;
            CLIK_GAIN(9) = K_trans;
            CLIK_GAIN(10) = K_trans;
            CLIK_GAIN(11) = K_trans;

            Control->SetPIDGain(Kp_.data, Kd_.data, Ki_.data, K_inf_.data);
            Control->SetTaskspaceGain(CLIK_GAIN, CLIK_GAIN_DUMY);
            alpha = 5.0;

            ControlMode = CTRLMODE_IDY_JOINT;
            ControlSubMode = SYSTEM_BEGIN;
            ControlMotion = MOVE_ZERO;
        }

        void update(const ros::Time &time, const ros::Duration &period) override
        {
            std::vector<double> &commands = *pub_buffer_.readFromRT();
            // ********* 0. Get states from gazebo *********
            // 0.1 sampling time
            double dt = period.toSec();

            clock_gettime(CLOCK_MONOTONIC, &begin);

            // 0.2 joint state
            for (int i = 0; i < n_joints_; i++)
            {
                q_(i) = joints_[i].getPosition();
                qdot_(i) = joints_[i].getVelocity();
                //torque_(i) = joints_[i].getEffort();
            }

            //----------------------
            // dynamics calculation
            //----------------------
            cManipulator->pKin->PrepareJacobian(q_.data);
            cManipulator->pDyn->PrepareDynamics(q_.data, qdot_.data);
            cManipulator->pKin->GetAnalyticJacobian(AJac);
            cManipulator->pKin->GetpinvJacobian(pInvJac);

            cManipulator->pKin->GetForwardKinematics(ForwardPos, ForwardOri, NumChain);
            cManipulator->pKin->GetAngleAxis(ForwardAxis, ForwardAngle, NumChain);
            cManipulator->pKin->GetInverseConditionNumber(InverseConditionNumber);

            q1_.data = q_.data.head(9);
            q1dot_.data = qdot_.data.head(9);

            q2_.resize(9);
            q2dot_.resize(9);
            q2_.data.tail(7) = q_.data.tail(7);
            q2_.data.head(2) = q_.data.head(2);
            q2dot_.data = qdot_.data.tail(7);
            q2dot_.data.head(2) = qdot_.data.head(2);

            fk_pos_solver_->JntToCart(q1_, x_[0]);
            fk_pos_solver1_->JntToCart(q2_, x_[1]);

            jnt_to_jac_solver_->JntToJac(q1_, J1_kdl_);
            jnt_to_jac_solver1_->JntToJac(q2_, J2_kdl_);

            MM = cManipulator->pKin->GetManipulabilityMeasure();
            manipulability_data();

            //cManipulator->pDyn->MG_Mat_Joint(M, G);
            //id_solver_->JntToMass(q1_, M_kdl_);
            //id_solver_->JntToCoriolis(q1_, q1dot_, C_kdl_);
            //id_solver_->JntToGravity(q1_, G_kdl_);
            //id_solver1_->JntToMass(q2_, M1_kdl_);
            //id_solver1_->JntToCoriolis(q2_, q2dot_, C1_kdl_);
            //id_solver1_->JntToGravity(q2_, G1_kdl_);

            ctr_obj_ = ControlSubMode;
            ik_mode_ = ControlMotion;

            if( ControlMode == CTRLMODE_CLIK )
            {
                //motion->TaskMotion(dx, dxdot, dxddot, targetpos, q_.data, qdot_.data, t, JointState, ControlMotion);
                //Control->CLIKTaskController(q_.data, qdot_.data, dx, dxdot, ft_sensor, torque, dt, ControlSubMode);
                Control->GetControllerStates(qd_.data, qd_dot_.data, ex_);

                if(ctr_obj_ == 6)
                    cManipulator->pKin->GetWDampedpInvLambda(wpInv_lambda);
            }
            else if( ControlMode == CTRLMODE_IDY_JOINT )
            {
                qd_dot_.data.setZero(16);
                motion->JointMotion(qd_.data, qd_dot_.data, qd_ddot_.data, targetpos, q_.data, qdot_.data, t, JointState, ControlMotion);
                Control->InvDynController(q_.data, qdot_.data, qd_.data, qd_dot_.data, qd_ddot_.data, torque, dt);
            }

            for (int i = 0; i < n_joints_; i++)
            {
                joints_[i].setCommand(torque(i));
            }

            clock_gettime(CLOCK_MONOTONIC, &end);
            // ********* 4. data 저장 *********
            if(t > InitTime)
                publish_data();

            // ********* 5. state 출력 *********
            print_state();

            t = t + dt;
        }

        void stopping(const ros::Time &time) override
        {
            ROS_INFO("Stop Closed-loop Inverse Dynamics Controller");
        }

        void manipulability_data()
        {
            const auto desired_Dualmanipulability =
                    manipulability_metrics::Ellipsoid{ { { (Eigen::Matrix<double, 6, 1>{} << 1, 0, 0, 0, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 1, 0, 0, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 1, 0, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 1, 0, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 1, 0).finished(), 1.0 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 0, 1).finished(), 1.0 } } };

            const auto desired_Singelmanipulability =
                    manipulability_metrics::Ellipsoid{ { { (Eigen::Matrix<double, 6, 1>{} << 1, 0, 0, 0, 0, 0).finished(), 0.2 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 1, 0, 0, 0, 0).finished(), 0.8 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 1, 0, 0, 0).finished(), 0.8 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 1, 0, 0).finished(), 0.9 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 1, 0).finished(), 1.7 },
                                                               { (Eigen::Matrix<double, 6, 1>{} << 0, 0, 0, 0, 0, 1).finished(), 1.7 } } };

            SingleMM[0] = sqrt((J1_kdl_.data * J1_kdl_.data.transpose()).determinant());
            SingleMM[1] = sqrt((J2_kdl_.data * J2_kdl_.data.transpose()).determinant());

            SingleMM_1[0] = sqrt((AJac.block(0,0,6,16)*AJac.block(0,0,6,16).transpose()).determinant());
            SingleMM_1[1] = sqrt((AJac.block(6,0,6,16)*AJac.block(6,0,6,16).transpose()).determinant());

            TOMM[0] = manipulability_metrics::inverseShapeDiscrepancy(desired_Singelmanipulability, J1_kdl_.data);
            TOMM[1] = manipulability_metrics::inverseShapeDiscrepancy(desired_Singelmanipulability, J2_kdl_.data);

            TOMM_1[0] = manipulability_metrics::inverseShapeDiscrepancy(desired_Singelmanipulability, AJac.block(0,0,6,16));
            TOMM_1[1] = manipulability_metrics::inverseShapeDiscrepancy(desired_Singelmanipulability, AJac.block(6,0,6,16));

            auto left_ellipsoid = manipulability_metrics::ellipsoidFromJacobian(J2_kdl_.data);
            auto right_ellipsoid = manipulability_metrics::ellipsoidFromJacobian(J1_kdl_.data);
            DAMM = std::max(manipulability_metrics::volumeIntersection(left_ellipsoid, J1_kdl_.data),
                                 manipulability_metrics::volumeIntersection(right_ellipsoid, J2_kdl_.data));
            TODAMM2 = manipulability_metrics::dualInverseShapeDiscrepancy(desired_Dualmanipulability, J2_kdl_.data, J1_kdl_.data);

            auto left_ellipsoid_1 = manipulability_metrics::ellipsoidFromJacobian(AJac.block(6,0,6,16));
            auto right_ellipsoid_1 = manipulability_metrics::ellipsoidFromJacobian(AJac.block(0,0,6,16));
            DAMM_1 = std::max(manipulability_metrics::volumeIntersection(left_ellipsoid_1, AJac.block(0,0,6,16)),
                            manipulability_metrics::volumeIntersection(right_ellipsoid_1, AJac.block(6,0,6,16)));
            TODAMM2_1 = manipulability_metrics::dualInverseShapeDiscrepancy(desired_Dualmanipulability, AJac.block(6,0,6,16), AJac.block(0,0,6,16));
        }

        void publish_data()
        {
            static int loop_count_ = 0;
            if(loop_count_ > 2)
            {
                if(state_pub_->trylock())
                {
                    state_pub_->msg_.header.stamp = ros::Time::now();
                    state_pub_->msg_.header.seq++;
                    for(size_t i=0; i<n_joints_; i++)
                    {
                        state_pub_->msg_.q[i] = q_.data(i);
                        state_pub_->msg_.qdot[i] = qdot_.data(i);
                        state_pub_->msg_.dq[i] = qd_.data(i);
                        state_pub_->msg_.dqdot[i] = qd_dot_.data(i);
                        state_pub_->msg_.torque[i] = torque(i);
                    }

                    for(int j=0; j<2; j++)
                    {
                        state_pub_->msg_.dx[j].orientation.x = dx(6*j);
                        state_pub_->msg_.dx[j].orientation.y = dx(6*j+1);
                        state_pub_->msg_.dx[j].orientation.z = dx(6*j+2);
                        state_pub_->msg_.dx[j].position.x = dx(6*j+3);
                        state_pub_->msg_.dx[j].position.y = dx(6*j+4);
                        state_pub_->msg_.dx[j].position.z = dx(6*j+5);

                        state_pub_->msg_.x[j].orientation.x = ForwardOri[j](0);
                        state_pub_->msg_.x[j].orientation.y = ForwardOri[j](1);
                        state_pub_->msg_.x[j].orientation.z = ForwardOri[j](2);
                        state_pub_->msg_.x[j].position.x = ForwardPos[j](0);
                        state_pub_->msg_.x[j].position.y = ForwardPos[j](1);
                        state_pub_->msg_.x[j].position.z = ForwardPos[j](2);

                        state_pub_->msg_.InverseConditionNum[j] = InverseConditionNumber[j];
                        state_pub_->msg_.SingleMM[j] = SingleMM[j];

                    }
                    state_pub_->msg_.MM = MM;
                    state_pub_->msg_.DAMM = DAMM;
                    state_pub_->msg_.TODAMM = TODAMM2;
                    state_pub_->msg_.Kp_T = K_trans;
                    state_pub_->msg_.Kp_R = K_rot;
                    state_pub_->msg_.lambda1[0] = wpInv_lambda[0](0);
                    state_pub_->msg_.lambda1[1] = wpInv_lambda[0](1);
                    state_pub_->msg_.lambda1[2] = wpInv_lambda[0](2);
                    state_pub_->msg_.lambda2[0] = wpInv_lambda[1](0);
                    state_pub_->msg_.lambda2[1] = wpInv_lambda[1](1);
                    state_pub_->msg_.lambda2[2] = wpInv_lambda[1](2);
                    state_pub_->unlockAndPublish();
                }
                loop_count_=0;
            }
            loop_count_++;
        }

        void print_state()
        {
            static int count = 0;
            if (count > 499)
            {
                printf("*********************************************************\n\n");
                printf("*** Calcutaion Time (unit: sec)  ***\n");
                printf("t_cal = %0.9lf\n", static_cast<double>(end.tv_sec - begin.tv_sec) + static_cast<double>(end.tv_nsec - begin.tv_nsec) / 1000000000.0);
                printf("*** Simulation Time (unit: sec)  ***\n");
                printf("t = %0.3lf\n", t);
                printf("IK mode:%d, CTRL_OBJ:%d\n", ik_mode_, ctr_obj_);
                printf("\n");

                printf("*** Command from Subscriber in Task Space (unit: m) ***\n");
                printf("*** States in Joint Space (unit: deg) ***\n");
                Control->GetPIDGain(aKp_, aKd_, aKi_);
                for(int i=0; i < n_joints_; i++)
                {
                    printf("[%s]:  \t", joint_names_[i].c_str());
                    printf("Kp:%0.3lf, Kd:%0.3lf,\t", aKp_(i), aKd_(i));
                    printf("q: %0.3lf,\t", q_.data(i) * R2D);
                    printf("dq: %0.3lf,\t", qd_.data(i) * R2D);
                    printf("qdot: %0.3lf,\t", qdot_.data(i) * R2D);
                    printf("dqdot: %0.3lf,\t", qd_dot_.data(i) * R2D);
                    printf("tau: %0.3f\n", torque(i));
                }

                printf("\nForward Kinematics:\n");
                for(int j=0; j<NumChain; j++)
                {
                    printf("no.%d, PoE: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n", j,
                           ForwardPos[j](0), ForwardPos[j](1),ForwardPos[j](2),
                           ForwardOri[j](0), ForwardOri[j](1), ForwardOri[j](2));
                    printf("no.%d, PoE_desired: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n", j,
                           dx(6*j+3),dx(6*j+4),dx(6*j+5),dx(6*j), dx(6*j+1), dx(6*j+2));
                    double a, b, g;
                    x_[j].M.GetEulerZYX(a, b, g);
                    printf("no.%d, DH: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n",
                           j, x_[j].p(0), x_[j].p(1),x_[j].p(2), g, b, a);
                    printf("no.%d, AngleAxis x: %0.2lf, y: %0.2lf, z: %0.2lf, Angle: %0.3lf\n",
                           j, ForwardAxis[j](0), ForwardAxis[j](1), ForwardAxis[j](2), ForwardAngle[j]);
                    printf("\n");
                }

                printf("FT Sensor(Right): torque_u:%0.3lf, torque_v:%0.3lf, torque_w:%0.3lf, force_x:%0.3lf, force_y:%0.3lf, force_z:%0.3lf\n",
                       ft_sensor(0), ft_sensor(1), ft_sensor(2),ft_sensor(3),ft_sensor(4),ft_sensor(5));
                printf("FT Sensor(Left): torque_u:%0.3lf, torque_v:%0.3lf, torque_w:%0.3lf, force_x:%0.3lf, force_y:%0.3lf, force_z:%0.3lf\n\n",
                       ft_sensor(6), ft_sensor(7), ft_sensor(8),ft_sensor(9),ft_sensor(10),ft_sensor(11));

                printf("Inverse Condition Number: Right:%0.5lf, Left:%0.5f\n", InverseConditionNumber[0], InverseConditionNumber[1]);
                printf("SingleMM: Right:%0.5lf, Left:%0.5lf :: Right:%0.5lf, Left:%0.5lf,\n", SingleMM[0], SingleMM[1], SingleMM_1[0], SingleMM_1[1]);
                printf("TOMM: Right:%0.5lf, Left:%0.5lf :: Right:%0.5lf, Left:%0.5lf\n", TOMM[0], TOMM[1], TOMM_1[0], TOMM_1[1]);
                printf("MM: %0.5lf\n", MM);
                printf("DAMM: %0.5lf :: DAMM: %0.5lf\n", DAMM, DAMM_1);
                printf("TODAMM: %0.5lf :: TODAMM: %0.5lf\n\n", TODAMM2, TODAMM2_1);

                printf("Right e(u):%0.3lf, e(v):%0.3lf, e(w):%0.3lf, e(x):%0.3lf, e(y):%0.3lf, e(z):%0.3lf\n",
                       ex_(0)*RADtoDEG, ex_(1)*RADtoDEG, ex_(2)*RADtoDEG, ex_(3), ex_(4), ex_(5));
                printf("Left e(u):%0.3lf, e(v):%0.3lf, e(w):%0.3lf, e(x):%0.3lf, e(y):%0.3lf, e(z):%0.3lf\n",
                       ex_(6)*RADtoDEG, ex_(7)*RADtoDEG, ex_(8)*RADtoDEG, ex_(9), ex_(10), ex_(11));
                printf("\n*********************************************************\n");
                count = 0;

                if(ctr_obj_ == 6)
                {
                    std::cout << "lambda[right]:" << std::endl;
                    std::cout << wpInv_lambda[0] << std::endl;
                    std::cout << "lambda[left]:" << std::endl;
                    std::cout << wpInv_lambda[1] << std::endl;
                }

                //std::cout << "J1:" <<std::endl;
                //std::cout << J1_kdl_.data << "\n"<< std::endl;
                //std::cout << "J2:" <<std::endl;
                //std::cout << J2_kdl_.data << "\n"<< std::endl;
                //std::cout << "Analytic Jacobian:" <<std::endl;
                //std::cout << AJac << "\n"<< std::endl;
            }
            count++;
        }

    private:
        // others
        double t=0.0;
        int ctr_obj_=0;
        int ik_mode_=0;
        double InitTime=0.0;

        unsigned char ControlMode;
        unsigned char ControlSubMode;
        unsigned char ControlMotion;
        unsigned char JointState;

        struct timespec begin, end;
        //Joint handles
        unsigned int n_joints_;
        std::vector<std::string> joint_names_;
        std::vector<hardware_interface::JointHandle> joints_;
        std::vector<urdf::JointConstSharedPtr> joint_urdfs_;

        // kdl
        KDL::Tree kdl_tree_;
        KDL::Chain kdl_chain_;
        KDL::Chain kdl_chain2_;

        KDL::JntSpaceInertiaMatrix M_kdl_, M1_kdl_;
        KDL::JntArray C_kdl_, C1_kdl_;
        KDL::JntArray G_kdl_, G1_kdl_;
        KDL::Vector g_kdl_;
        Eigen::VectorXd g_vec_collect;
        Eigen::MatrixXd g_mat_collect;
        Eigen::MatrixXd M_mat_collect;

        KDL::Jacobian J1_kdl_, J2_kdl_;

        // kdl solver
        boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_, fk_pos_solver1_; //Solver to compute the forward kinematics (position)
        boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_, jnt_to_jac_solver1_; //Solver to compute the jacobian
        boost::scoped_ptr<KDL::ChainDynParam> id_solver_, id_solver1_;               // Solver To compute the inverse dynamics

        MatrixXd M;
        VectorXd G;
        VectorXd C;

        Vector3d ForwardPos[2];
        Vector3d ForwardOri[2];
        int NumChain=2;
        Vector3d ForwardAxis[2];
        double ForwardAngle[2];
        double InverseConditionNumber[2];
        double SingleMM[2], SingleMM_1[2];
        double TOMM[2], TOMM_1[2];
        double MM, MM_1;
        double DAMM, DAMM_1;
        double TODAMM2, TODAMM2_1;

        // kdl and Eigen Jacobian
        Eigen::MatrixXd pInvJac;
        Eigen::MatrixXd AJac;
        Eigen::MatrixXd BlockpInvJac;
        Eigen::MatrixXd ScaledJac;
        Eigen::MatrixXd dampedpInvJac;
        Eigen::MatrixXd WdampedpInvJac;
        VectorXd wpInv_lambda[2];

        // Joint Space State
        KDL::JntArray qd_;
        KDL::JntArray qd_dot_;
        KDL::JntArray qd_ddot_;
        KDL::JntArray qd_old_;
        KDL::JntArray q_, q1_, q2_;
        KDL::JntArray qdot_, q1dot_, q2dot_;
        Eigen::VectorXd q0dot;
        double alpha;
        Eigen::VectorXd ft_sensor;
        Eigen::VectorXd torque;
        Eigen::VectorXd targetpos;

        // Task Space State
        // ver. 01
        KDL::Frame xd_[2];
        KDL::Frame x_[2];
        KDL::Twist ex_temp_;

        // KDL::Twist xd_dot_, xd_ddot_;
        Eigen::VectorXd ex_;
        Eigen::VectorXd ex_dot_;
        Eigen::VectorXd dx;
        Eigen::VectorXd dxdot;
        Eigen::VectorXd dxddot;

        // Input
        KDL::JntArray x_cmd_;

        // gains
        KDL::JntArray Kp_, Ki_, Kd_, K_inf_;
        Eigen::VectorXd aKp_, aKi_, aKd_, aK_inf_;
        double K_trans, K_rot;
        Eigen::VectorXd CLIK_GAIN, CLIK_GAIN_DUMY;

        // publisher
        realtime_tools::RealtimeBuffer<std::vector<double>> pub_buffer_;
        boost::scoped_ptr<realtime_tools::RealtimePublisher<dualarm_controller::TaskCurrentState>> state_pub_;

        // subscriber
        ros::Subscriber sub_x_cmd_;
        ros::Subscriber sub_ft_sensor_R, sub_ft_sensor_L;

        std::shared_ptr<SerialManipulator> cManipulator;
        std::unique_ptr<HYUControl::Controller> Control;
        std::unique_ptr<HYUControl::Motion> motion;
    };
}

PLUGINLIB_EXPORT_CLASS(dualarm_controller::ClosedLoopIK_Control,controller_interface::ControllerBase)