//
//map_alignment.launch をつかってパラメータ調整する
//
//

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

#include <std_msgs/String.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

#include "std_msgs/Int32.h"
#include "std_msgs/Float64.h"

#include <math.h>

#include <local_tool/filters.hpp>
#include <local_tool/mathematics.hpp>
#include <local_tool/registration.hpp>

typedef pcl::PointXYZINormal PointN;
typedef pcl::PointXYZ PointX;
typedef pcl::PointCloud<PointN>::Ptr CloudNPtr;
typedef pcl::PointCloud<PointX>::Ptr CloudXPtr;

using namespace std;
using namespace Eigen;

CloudXPtr input_cloud (new pcl::PointCloud<PointX>);
CloudXPtr limit_lidar (new pcl::PointCloud<PointX>);
CloudXPtr limit_input_cloud (new pcl::PointCloud<PointX>);
CloudXPtr filtered_laser_cloud (new pcl::PointCloud<PointX>);
CloudXPtr before_laser_cloud (new pcl::PointCloud<PointX>);
CloudXPtr local_map_cloud (new pcl::PointCloud<PointX>);
CloudXPtr target_map_cloud (new pcl::PointCloud<PointX>);
CloudXPtr filtered_map_cloud (new pcl::PointCloud<PointX>);
CloudXPtr output_cloud (new pcl::PointCloud<PointX>);

class Align{
	private:
		ros::NodeHandle n;
		ros::Rate r;
		ros::Subscriber velo_sub;
		ros::Subscriber odom_sub;
		ros::Publisher ndt_pub;
		ros::Publisher vis_voxel_pub;
		ros::Publisher vis_map_pub;

		sensor_msgs::PointCloud2 vis_laser_voxel;
		sensor_msgs::PointCloud2 vis_map;
		nav_msgs::Odometry odo;
		nav_msgs::Odometry odo_pub;

		float x_now,y_now,yaw_now;
		float z_now;

		float map_limit,laser_limit;//map・laserの距離
		string map_file;

		float laser_size,map_size;//voxel
		bool flag;//lidar
		bool flag_laser;//lidar
		double l_roll, l_pitch, l_yaw;//角度etc

		string LIDAR_TOPIC;
		string ODOM_TOPIC;
		string NDT_TOPIC;

	public:
		Align(ros::NodeHandle& n);
		void first_laser();//mapの点群を扱う
		void maptolidar();//位置合わせを行う

		void odomCallback(const nav_msgs::Odometry::Ptr msg);
		void velodyneCallback(const sensor_msgs::PointCloud2ConstPtr input);//velodyneの点群を扱う
		bool spin()
		{
			ros::Rate loop_rate(r);

			while(ros::ok()){
				if(flag_laser)first_laser();
				if(flag)maptolidar();
				ros::spinOnce();
				loop_rate.sleep();
			}
			return true;
		}
};

Align::Align(ros::NodeHandle& n) :
	r(10),
	flag(false),
	flag_laser(true)
{

    n.param("laser_voxel_size",laser_size, 0.0f);
    n.param("map_voxel_size",map_size, 0.0f);
    n.param("map_limit",map_limit, 0.0f);
    n.param("laser_limit",laser_limit, 0.0f);
    n.param("init_x",x_now, 0.0f);
    n.param("init_y",y_now, 0.0f);
    n.param("init_z",z_now, 0.0f);
    n.param("init_yaw",yaw_now, 0.0f);
    n.param("lidar_topic",LIDAR_TOPIC, {0});
    n.param("odom_topic",ODOM_TOPIC, {0});
    n.param("ndt_topic",NDT_TOPIC, {0});
    // n.getParam("ndt_topic",NDT_TOPIC);
	n.getParam("map/d_kan_around",map_file);
	// n.getParam("map/d_kan_indoor",map_file);

	velo_sub = n.subscribe(LIDAR_TOPIC, 1000, &Align::velodyneCallback, this);
	odom_sub = n.subscribe(ODOM_TOPIC, 1000, &Align::odomCallback, this);

	ndt_pub = n.advertise<nav_msgs::Odometry>(NDT_TOPIC, 1000);
	vis_voxel_pub = n.advertise<sensor_msgs::PointCloud2>("/ndt_result", 1000);
	vis_map_pub = n.advertise<sensor_msgs::PointCloud2>("/vis_map", 1000);

	// cout << "-------PARAMETARS----------" << endl;
	cout << "map_name：" << map_file << endl;

	l_roll = l_pitch = l_yaw = 0;

}

//現在地を知る
void
Align::odomCallback(const nav_msgs::Odometry::Ptr msg){

    odo.pose = msg->pose;

	x_now = msg->pose.pose.position.x;
	y_now = msg->pose.pose.position.y;
	z_now = 0;

	yaw_now = msg->pose.pose.orientation.z;
}

//lidar情報
void
Align::velodyneCallback(const sensor_msgs::PointCloud2ConstPtr input){

	flag = true;
	float d = 0;

	pcl::fromROSMsg(*input,*input_cloud);

	limit_input_cloud->points.clear();

    size_t velodyne_size = input_cloud->points.size();
	for(size_t i = 0; i < velodyne_size; i++){
		pcl::PointXYZ temp_point;
		temp_point.x = input_cloud->points[i].x;
		temp_point.y = input_cloud->points[i].y;
		temp_point.z = input_cloud->points[i].z + z_now;

        d = distance(temp_point.x , temp_point.y);

        if(1.5 <= d &&d <= laser_limit){//つくば

			limit_input_cloud->points.push_back(temp_point);

		}

	}

	voxel_grid(laser_size,limit_input_cloud,filtered_laser_cloud);

}

//参照データの読み込み
void
Align::first_laser()
{
	before_laser_cloud = filtered_laser_cloud;

	flag_laser = false;
}

///メイン処理
void
Align::maptolidar()
{
	cout << "レーザから得た Filtered cloud contains " << filtered_laser_cloud->size ()<< endl;

	Matrix4f a;
	// a = (local_map_cloud,filtered_laser_cloud,output_cloud,odo);
	a = registration_icp(before_laser_cloud,filtered_laser_cloud);

	calc_rpy(a,l_roll,l_pitch,l_yaw);

	odo_pub.pose.pose.position.x = x_now + a(0, 3);
	odo_pub.pose.pose.position.y = y_now + a(1, 3);
	odo_pub.pose.pose.orientation.z = l_yaw;

	ndt_pub.publish(odo_pub);

	pcl::toROSMsg(*output_cloud , vis_laser_voxel);
	pcl::toROSMsg(*local_map_cloud , vis_map);

    vis_laser_voxel.header.frame_id = "/map"; //laserのframe_id
	vis_map.header.frame_id = "/map"; //laserのframe_id

	vis_laser_voxel.header.stamp = ros::Time::now(); //laserのframe_id
	vis_map.header.stamp = ros::Time::now(); //laserのframe_id

	vis_voxel_pub.publish(vis_laser_voxel);
	vis_map_pub.publish(vis_map);

	before_laser_cloud = filtered_laser_cloud;
}

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

	cout<<"-----alignment start-------"<<endl;

	Align align(n);

	align.spin();

	return 0;
}


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

  Defines the as_simd metafunction for SSE2 like extensions

  @copyright 2012 - 2015 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_AS_SIMD_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_AS_SIMD_HPP_INCLUDED

#include <boost/simd/arch/x86/sse1/as_simd.hpp>
#include <boost/dispatch/meta/is_natural.hpp>

namespace boost { namespace simd
{
  template<typename T> struct logical;

  namespace ext
  {
    template<> struct as_simd<double, boost::simd::sse_>
    {
      using type = __m128d;
    };

    template<typename T>
    struct as_simd< T, boost::simd::sse_
                  , typename std::enable_if<boost::dispatch::is_natural<T>::value>::type
                  >
    {
      using type = __m128i;
    };
  }
} }

#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include <boost/test/tools/old/interface.hpp>
#include <ostream>
#include <sstream>
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE statetracker_test

// Standard includes
#include <fstream>

// Third party includes
#include <boost/test/unit_test.hpp>

// VOTCA includes
#include <votca/tools/eigenio_matrixmarket.h>

// Local VOTCA includes
#include "votca/xtp/statetracker.h"
#include <libint2/initialize.h>
using namespace votca::xtp;
using namespace std;
BOOST_AUTO_TEST_SUITE(statetracker_test)
BOOST_AUTO_TEST_CASE(osc) {
  libint2::initialize();
  Orbitals orb;
  orb.QMAtoms().LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                             "/statetracker/molecule.xyz");
  orb.setDFTbasisName(std::string(XTP_TEST_DATA_FOLDER) +
                      "/statetracker/3-21G.xml");
  Logger log;
  orb.setBasisSetSize(17);
  orb.setNumberOfOccupiedLevels(4);
  orb.setBSEindices(0, 16);
  orb.setTDAApprox(true);

  Eigen::MatrixXd& MOs = orb.MOs().eigenvectors();

  orb.MOs().eigenvalues() = Eigen::VectorXd::Ones(17);
  MOs = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/statetracker/MOs.mm");

  Eigen::VectorXd se_ref = Eigen::VectorXd::Zero(3);
  se_ref << 0.107455, 0.107455, 0.107455;
  orb.BSESinglets().eigenvalues() = se_ref;

  // reference coefficients
  Eigen::MatrixXd spsi_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/statetracker/spsi_ref.mm");

  orb.BSESinglets().eigenvectors() = spsi_ref;
  orb.CalcCoupledTransition_Dipoles();

  {
    votca::tools::Property prop;
    prop.LoadFromXML(std::string(XTP_TEST_DATA_FOLDER) +
                     "/statetracker/statetracker.xml");
    StateTracker tracker;
    tracker.setLogger(&log);
    QMState s("s1");
    tracker.setInitialState(s);
    tracker.Initialize(prop.get("statetracker"));
    QMState newstate = tracker.CalcState(orb);
    BOOST_CHECK_EQUAL(newstate.Type().ToString(), "s");
    BOOST_CHECK_EQUAL(newstate.StateIdx(), 1);
  }
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(readwrite_hdf5) {
  votca::tools::Property prop;
  prop.LoadFromXML(std::string(XTP_TEST_DATA_FOLDER) +
                   "/statetracker/statetracker2.xml");

  Logger log;
  StateTracker tracker;
  tracker.setLogger(&log);
  QMState s("s1");
  tracker.setInitialState(s);
  tracker.Initialize(prop.get("statetracker"));
  tracker.PrintInfo();
  std::stringstream ss;
  ss << log << std::flush;

  std::string output1 = ss.str();

  {
    CheckpointFile f("statetracker_test.hdf5");
    CheckpointWriter w = f.getWriter();
    tracker.WriteToCpt(w);
  }
  StateTracker tracker2;
  CheckpointFile f("statetracker_test.hdf5");
  CheckpointReader r = f.getReader();
  tracker2.ReadFromCpt(r);
  tracker2.setLogger(&log);
  tracker2.PrintInfo();
  std::stringstream ss2;
  ss2 << log << std::flush;
  BOOST_CHECK_EQUAL(tracker.InitialState().ToString(),
                    tracker2.InitialState().ToString());

  BOOST_CHECK_EQUAL(output1, ss2.str());
}

BOOST_AUTO_TEST_SUITE_END()


//          Copyright Rein Halbersma 2010-2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <core/board/sequence.hpp>
#include <dctl/core/action.hpp>
#include <dctl/core/board.hpp>          // international
#include <dctl/core/rules.hpp>
#include <dctl/core/state.hpp>
#include <boost/mpl/vector.hpp>         // vector
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE
#include <iostream>

using namespace dctl::core;

BOOST_AUTO_TEST_SUITE(SetupLayout)

using RSequence = boost::mpl::vector
<
        checkers,
        czech,
        frisian,
        international,
        italian,
        pool,
        russian,
        spanish,
        thai
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(initial, T, BoardSequence)
{
        auto const s = basic_state<international, T>::initial();
        std::cout << diag << s;
        std::cout << "W = " << T::width << ", H = " << T::height << ", P = " << T::coloring << ", bits = " << T::bits() << "\n\n";
}

BOOST_AUTO_TEST_CASE_TEMPLATE(ActionSize, T, RSequence)
{
        using A = basic_action<T, basic_board<international>>;
        std::cout << "sizeof(Action) = " << sizeof(A) << "\n";
}

BOOST_AUTO_TEST_CASE_TEMPLATE(StateSize, T, RSequence)
{
        using state = basic_state<T, basic_board<international>>;
        std::cout << "sizeof(state) = " << sizeof(state) << "(" << sizeof(square_t<basic_mask<board_t<state>>>) << ")" << "\n";
}

BOOST_AUTO_TEST_CASE(Grid)
{
        auto const d = detail::dimensions{1,1,0};
        auto const i = detail::InnerGrid{d};
        auto const o = detail::bit_layout{i, 2};
        std::cout << "W = " << i.width() << ", H = " << i.height() << ", P = " << i.coloring() << ", bits = " << i.size() << "\n\n";
        std::cout << "W = " << o.width() << ", H = " << o.height() << ", P = " << o.coloring() << ", bits = " << o.size() << "\n\n";
}

BOOST_AUTO_TEST_SUITE_END()


// -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
/**
   c) Tokuo Tsuji (Kyushu univ./AIST) and Kensuke Harada (AIST)
*/

#include <fstream>
#include <string>
#include <iostream>

#include <math.h>

#include <algorithm>
#include <time.h>
#ifndef WIN32
#include <sys/resource.h>
#endif

#include <boost/filesystem.hpp>

#include <cnoid/JointPath>	/* modified by qtconv.rb 0th rule*/
#include <cnoid/MessageView>	/* modified by qtconv.rb 0th rule*/
#include <cnoid/ExecutablePath>

//#define DEBUG_MODE
//#define SET_TOLERANCE_MODE

#include "GraspController.h"
#include "PlaceController.h"
#include "readtext.h"
#include "VectorMath.h"
#include "ForceClosureTest.h"

#include "GraspPluginManager.h"

//#define BOOST_PYTHON_STATIC_LIB
//#ifndef NDEBUG
//#define BOOST_DEBUG_PYTHON
//#endif
#include <boost/python.hpp>
#include <boost/make_shared.hpp>

#define deg2Rad(x)   ((x)*(3.141596)/(180.0))
#define m_pi    (3.141596)

namespace grasp{

void _initrand(){
#ifdef WIN32
	  srand((unsigned)time(NULL));
#else
	  srand48(time(0));
#endif
	}

#ifdef WIN32
double getrusage_sec() {
	return clock();
}
#else
double getrusage_sec() {
	struct rusage t;
	struct timeval tv;
	getrusage(RUSAGE_SELF, &t);
	tv = t.ru_utime;
	return tv.tv_sec + (double)tv.tv_usec*1e-6;
}
#endif

class PythonConverter{
	public:
		static PythonConverter* instance(){
			static PythonConverter* instance = new PythonConverter;
			return instance;
		}
		static cnoid::Vector3 setVector3(double a, double b, double c){
			return cnoid::Vector3(a,b,c);
		}
};

}

BOOST_PYTHON_MODULE( grasp )
{
using namespace boost::python;
 //   class_<grasp::GraspController, boost::noncopyable>("GraspController", no_init)
 //       .def("instance", &grasp::GraspController::instance, return_value_policy<reference_existing_object>()).staticmethod("instance")
 //   ;
    class_<grasp::PlanBase, boost::noncopyable>("PlanBase", no_init)
        .def("instance", &grasp::PlanBase::instance, return_value_policy<reference_existing_object>()).staticmethod("instance")
        .def("initial", &grasp::PlanBase::initial)
//        .def("doGraspPlanning", &grasp::PlanBase::doGraspPlanning)
 //       .def("doPlacePlanning", &grasp::PlanBase::doPlacePlanning)
  //      .def("doPickAndPlacePlanning", &grasp::PlanBase::doPickAndPlacePlanning)
		.add_property("stopFlag", &grasp::PlanBase::stopFlag) //test
    ;
	class_<cnoid::BodyItemPtr>("BodyItemPtr")
	;
	class_<cnoid::Vector3>("Vector3")
	;
    class_<grasp::PythonConverter>("PythonConverter")
		.def("Vector3", &grasp::PythonConverter::setVector3).staticmethod("Vector3")
	;
}

using namespace std;
using namespace cnoid;
using namespace grasp;
using namespace boost;

MotionState PlanBase::getMotionState(double time){
	MotionState ret;

	ret.jointSeq = VectorXd(body()->numJoints());
	for(int i=0;i<body()->numJoints();i++)
        ret.jointSeq[i] = body()->joint(i)->q();

    ret.pos = body()->link(0)->p();
    ret.rpy = rpyFromRot(body()->link(0)->R());

	ret.graspingState = getGraspingState();
	ret.graspingState2 = getGraspingState2();
	if( getGraspingState()==GRASPING ){
		ret.objectPalmPos = targetArmFinger->objectPalmPos;
		ret.objectPalmRot = targetArmFinger->objectPalmRot;
	}
	ret.objectContactState = getObjectContactState();
	ret.pathPlanDOF = pathPlanDOF;
	ret.tolerance = tolerance;
	ret.time = time;
	ret.id = motionId;
	return ret;
}

void PlanBase::setMotionState(MotionState gm){
	for(int i=0;i<body()->numJoints();i++){
        body()->joint(i)->q() = gm.jointSeq[i];
	}
    body()->link(0)->p() = gm.pos;
    body()->link(0)->R() = rotFromRpy(gm.rpy);

	setGraspingState(gm.graspingState);
	setGraspingState2(gm.graspingState2);
	if( getGraspingState()==GRASPING ){
		targetArmFinger->objectPalmPos = gm.objectPalmPos;
		targetArmFinger->objectPalmRot = gm.objectPalmRot;
	}
	setObjectContactState(gm.objectContactState);
	pathPlanDOF = gm.pathPlanDOF;
	setTolerance(gm.tolerance);
	calcForwardKinematics();
	motionId = gm.id;
}

PlanBase::PlanBase()  : 	os (MessageView::mainInstance()->cout() )
{
//	bodyItemRobot = NULL;
//	bodyItemGRC = NULL;
	targetObject = NULL;
	targetArmFinger=NULL;
	stopFlag = false;
//	refSize = refCsSize = 0;
//	arm = NULL;
//	fingers = NULL;
	graspingState = NOT_GRASPING;
	graspingState2 = NOT_GRASPING;
	tolerance = 0.0;
	ulimitMap=Vector3(1,1,1);
	llimitMap = Vector3(-1,-1,-1);
	doInitialCollision=true;
	useObjectSafeBoundingBox = false;
	useRobotSafeBoundingBox = false;
	boundingBoxSafetySize = Vector3(0.005,0.005,0.005);
	motionId = -1;

    if ( PyImport_AppendInittab( (char *)"grasp", initgrasp ) == -1 ) {
        MessageView::mainInstance()->put("faild init Grasp Module");
//        return;
    }
}

PlanBase::~PlanBase() {
	RemoveAllPointCloudEnvironment();
}

PlanBase* PlanBase::instance(PlanBase *gc) {
	static PlanBase* instance = (gc) ? gc : new PlanBase();
	if(gc) instance = gc;
	return instance;
}

TargetObject::TargetObject(cnoid::BodyItemPtr bodyItem){
	bodyItemObject = bodyItem;
	object = bodyItemObject->body()->link(0);
    objVisPos = object->p();
    objVisRot = object->R();
    objMass = object->m();
	offsetApplied = false;
}

void PlanBase::SetGraspedObject(cnoid::BodyItemPtr bodyItem){

	targetObject = new TargetObject(bodyItem); //shoud be chaged;

	Box& OCP = targetObject->OCP;
	calcBoundingBox(object()->coldetModel(), OCP.edge, OCP.p, targetObject->objCoM_, OCP.R);
	if(targetArmFinger){
		for(int i=0;i<nFing();i++) fingers(i)->coldetLinkPair(targetObject->bodyItemObject);
#ifdef  CNOID_10_11_12_13
		arm()->palmObjPair = new ColdetLinkPair(palm(),object() );
#else
		arm()->palmObjPair = make_shared<ColdetLinkPair>(body(), palm(), bodyItem->body(), object() );
#endif
	}
	string tagId = bodyItem->name();
	if(objTag2Item.find(tagId) == objTag2Item.end()){
		objTag2Item.insert( pair <string,BodyItemPtr>(tagId, bodyItem) );
	}
	setObjectContactState(ON_ENVIRONMENT) ;

	targetObject->safeBoundingBox = ColdetPairData::getSafeBoundingBox(object()->coldetModel(), boundingBoxSafetySize);

	initialCollision();
}

ArmFingers::ArmFingers(cnoid::BodyItemPtr bodyItem, const YamlMapping& gSettings) :  os (MessageView::mainInstance()->cout() )
{
	bodyItemRobot = bodyItem;

#ifdef CNOID_10_11
	boost::filesystem::path robotfullpath( bodyItemRobot->modelFilePath() );
#else
	boost::filesystem::path robotfullpath( bodyItemRobot->lastAccessedFilePath() );
#endif

	cout << robotfullpath.string() << endl;
	bodyItemRobotPath = boost::filesystem::path (robotfullpath.branch_path()).string();
	dataFilePath = bodyItemRobotPath + "/data/";

	//READ YAML setting

	palm = bodyItemRobot->body()->link(gSettings["palm"].toString());
	base = bodyItemRobot->body()->link(gSettings["base"].toString());

	const YamlSequence& tips = *gSettings["fingerEnds"].toSequence();

	nFing = tips.size();
	fingers = new FingerPtr[nFing];

	arm=NULL;
	for (int i = 0;i < tips.size();i++) {
		fingers[i]=NULL;
	}

	if( gSettings.find("GrasplotPluginDir")->type() == YAML_SCALAR ){
#ifdef WIN32
		string pluginPath =  cnoid::executableTopDirectory() + string("\\bin\\") + bodyItemRobot->name() + string("\\");
#else
		string pluginPath = bodyItemRobotPath + "/" + gSettings["GrasplotPluginDir"].toString();
#endif
		os << "Grasplot Plugin Path " << pluginPath << endl;
		gPluginManager.scanPluginFiles(pluginPath);

		arm = (Arm *)gPluginManager.loadGrasplotPlugin(bodyItemRobot->body(),base,palm, "Arm");

		for (int i = 0;i < tips.size();i++) {
			if(!fingers[i]) fingers[i] = (Finger *)gPluginManager.loadGrasplotPlugin
					(bodyItemRobot->body(), palm, bodyItemRobot->body()->link(tips[i].toString()), "Finger");
		}
	}
	if(!arm){
		arm = new Arm(bodyItemRobot->body(),base, palm);
	}
	for (int i = 0;i < tips.size();i++) {
		if(!fingers[i]) fingers[i] = new Finger(bodyItemRobot->body(), palm, bodyItemRobot->body()->link(tips[i].toString()) );
		fingers[i]->number = i;
	}

	if( gSettings.find("dataFilePath")->type() == YAML_SCALAR ){
		dataFilePath = bodyItemRobotPath + "/" + gSettings["dataFilePath"].toString() +"/";
	}

	if( gSettings.find("armStandardPose")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["armStandardPose"].toSequence();
		for(int i=0;i<list.size();i++){
			arm->armStandardPose.push_back(list[i].toDouble());
		}
	}

	if( gSettings.find("armFinalPose")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["armFinalPose"].toSequence();
		for(int i=0;i<list.size();i++){
			arm->armFinalPose.push_back(list[i].toDouble());
		}
	}

	if( gSettings.find("fingerOpenPose")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["fingerOpenPose"].toSequence();
		int j=0;
		int k=0;
		for(int i=0;i<list.size();i++){
			if(i>=k+fingers[j]->fing_path->numJoints()){
				k += fingers[j]->fing_path->numJoints();
				j++;
			}
			fingers[j]->fingerOpenPose.push_back(list[i].toDouble());
		}
	}

	if( gSettings.find("fingerOpenPoseOffset")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["fingerOpenPoseOffset"].toSequence();
		int j=0;
		int k=0;
		for(int i=0;i<list.size();i++){
			if(i>=k+fingers[j]->fing_path->numJoints()){
				k += fingers[j]->fing_path->numJoints();
				j++;
			}
			fingers[j]->fingerOpenPoseOffset.push_back(list[i].toDouble());
		}
	}

	if( gSettings.find("fingerCloseOffset")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["fingerCloseOffset"].toSequence();
		int j=0;
		int k=0;
		for(int i=0;i<list.size();i++){
			if(i>=k+fingers[j]->fing_path->numJoints()){
				k += fingers[j]->fing_path->numJoints();
				j++;
			}
			fingers[j]->fingerCloseOffset.push_back(list[i].toDouble());
		}
	}

	if( gSettings.find("fingerOffset")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["fingerOffset"].toSequence();
		for(int j=0; j<nFing; j++)
			fingers[j]->offset = list[0].toDouble();
	}

	if( gSettings.find("pythonInterface")->type() == YAML_SCALAR ){
		pythonInterface = bodyItemRobotPath + "/" + gSettings["pythonInterface"].toString();
	}else{
		pythonInterface = "/NULL";
	}

	vector <InterLink> & interLinkList = PlanBase::instance()->interLinkList;
	if( gSettings.find("interlink")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["interlink"].toSequence();
		for(int i=0;i<list.size();i++){
			const YamlSequence& ilist = *list[i].toSequence();
			Link* master = bodyItemRobot->body()->link(ilist[0].toString());
			double baseratio = ilist[1].toDouble();
			for(int j=1;j<ilist.size()/2;j++){
				InterLink temp;
				temp.master = master;
				temp.slave = bodyItemRobot->body()->link(ilist[2*j].toString());
				temp.ratio = ilist[2*j+1].toDouble()/baseratio;
				interLinkList.push_back(temp);
			}

		}
	}
	if( gSettings.find("approachOffset")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["approachOffset"].toSequence();
		for(int i=0;i<list.size();i++){
			arm->approachOffset[i] = list[i].toDouble();
		}
	}

	if( gSettings.find("selfContactPair")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["selfContactPair"].toSequence();
		if(list[0].type() == YAML_SCALAR){
			for(int i=0;i<list.size()/2;i++){
				contactLinks.insert ( make_pair ( list[i*2].toString(), list[i*2+1].toString() ) );
			}
		}
		if(list[0].type() == YAML_SEQUENCE){
			for(int i=0;i<list.size();i++){
				const YamlSequence& plist = *list[i].toSequence();
				for(int j=0;j<plist.size();j++){
					for(int k=j+1;k<plist.size();k++){
						contactLinks.insert ( make_pair (plist[j].toString(), plist[k].toString() )  );
					}
				}
			}

		}
	}
	if( gSettings.find("movableArea")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["movableArea"].toSequence();
		const YamlSequence& ilist = *list[0].toSequence();
		PlanBase::instance()->llimitMap = Vector3 ( ilist[0].toDouble(),ilist[1].toDouble(),ilist[2].toDouble() );
		const YamlSequence& ilist1 = *list[1].toSequence();
		PlanBase::instance()->ulimitMap = Vector3 ( ilist1[0].toDouble(),ilist1[1].toDouble(),ilist1[2].toDouble() );
	}

	cout << "llimit" << PlanBase::instance()->llimitMap.transpose() << " ";
	cout << "ulimit" << PlanBase::instance()->ulimitMap.transpose() << endl;

	if( gSettings.find("InterObject")->type() == YAML_SEQUENCE ){
		const YamlSequence& list = *gSettings["InterObject"].toSequence();
		for(int i=0;i<list.size();i++){
			InterObject tempo;
			const YamlSequence& ilist = *list[i].toSequence();
			tempo.master  = bodyItemRobot->body()->link(ilist[0].toString());

			BodyItemPtr temp = new BodyItem();
			if( !temp->loadModelFile(dataFilePath +ilist[1].toString()) ){
				os << "modelLoadError: " << dataFilePath +ilist[1].toString() << endl;
				break;
			}
			temp->setName(ilist[1].toString());
			bodyItemRobot->addChildItem (temp);
			tempo.slaveItem = temp;
			const YamlSequence& vlist = *ilist[2].toSequence();
			tempo.relativePos = Vector3 ( vlist[0].toDouble(),vlist[1].toDouble(),vlist[2].toDouble() );
			const YamlSequence& vlist2 = *ilist[3].toSequence();
			tempo.relativeRot << vlist2[0].toDouble(), vlist2[1].toDouble(), vlist2[2].toDouble(), vlist2[3].toDouble(), vlist2[4].toDouble(), vlist2[5].toDouble(), vlist2[6].toDouble(), vlist2[7].toDouble(), vlist2[8].toDouble();
			tempo.setInterObject();
			tempo.type = InterObject::ROBOT;
			PlanBase::instance()->interObjectList.push_back(tempo);
		}
	}

	if( gSettings.find("mu")->type() == YAML_SCALAR){
		mu = gSettings["mu"].toDouble();
	}else{
		mu = 0.5;
	}

	if( gSettings.find("fmax")->type() == YAML_SCALAR){
		fmax = gSettings["fmax"].toDouble();
	}else{
		fmax = 10;
	}

	if( gSettings.find("hmax")->type() == YAML_SCALAR){
		hmax = gSettings["hmax"].toDouble();
	}else{
		hmax = 0.005;
	}

	handJoint = new LinkTraverse(palm);
	nHandLink = handJoint->numLinks();

	if (gSettings.find("name")->type() == YAML_SCALAR ){
		name = gSettings["name"].toString();
	}else{
		static int i=0;
		stringstream namenum;
		namenum << arm << i;
		name = namenum.str();
	}

}

bool PlanBase::SetGraspingRobot(cnoid::BodyItemPtr bodyItem_){
	//setting robot

	armsList.clear(); //Temoporary;  will delete menbers
	interLinkList.clear();
	targetArmFinger = NULL;

	//READ YAML setting
	if( bodyItem_->body()->info()->find("graspPluginSetting")->type() == YAML_SEQUENCE){ // multi arm
		const YamlSequence& glist = *(*bodyItem_->body()->info())["graspPluginSetting"].toSequence();
		for(int i=0;i<glist.size();i++){
			const YamlMapping& gSettings = *glist[i].toMapping();
			if ( gSettings.isValid() && !gSettings.empty()) {
				targetArmFinger = new ArmFingers(bodyItem_, gSettings);
				armsList.push_back(targetArmFinger);
			}
		}
	}
	else{ // single arm
		const YamlMapping& gSettings = *bodyItem_->body()->info()->findMapping("graspPluginSetting");
		if ( gSettings.isValid() && !gSettings.empty()) {
			targetArmFinger = new ArmFingers(bodyItem_, gSettings);
			armsList.push_back(targetArmFinger);
		}
	}

	targetArmFinger = armsList[0];

	if(targetArmFinger == NULL){
		os << "ERROR graspPluginSetting is not found in yaml" << endl;
		return false;
	}

	robTag2Arm.clear();
	for(int i=0;i<armsList.size();i++){
		armsList[i]->id = i;
		string tagId = armsList[i]->name;
		if(robTag2Arm.find(tagId) != robTag2Arm.end()){
			os << "Error: the tagId is already recorded " << tagId << endl;
			continue;
		}else{
			robTag2Arm.insert( pair <string,ArmFingers*>(tagId, armsList[i]));
		}
	}

	os  << bodyItem_->name() << " has " <<armsList.size() << " arm(s) "<< endl;

	if(targetObject){
		for(int i=0;i<nFing();i++) fingers(i)->coldetLinkPair(targetObject->bodyItemObject);
#ifdef  CNOID_10_11_12_13
		arm()->palmObjPair = new ColdetLinkPair(palm(),object() );
#else
		arm()->palmObjPair = make_shared<ColdetLinkPair>(body(), palm(), targetObject->bodyItemObject->body(), object() );
#endif
	}

	bodyItemRobot()->body()->calcForwardKinematics();

	graspMotionSeq.clear();
	setGraspingState(NOT_GRASPING);
	setGraspingState2(NOT_GRASPING);

	robotSelfPairs.clear();
	for(unsigned int i=0;i<bodyItemRobot()->body()->numLinks();i++){ // If initial position is not collided, it is stored as
		for(unsigned int j=i+1;j<bodyItemRobot()->body()->numLinks();j++){
			bool pass = false;
			pair<multimap<string, string>::iterator, multimap<string, string>::iterator> ppp;
			ppp = targetArmFinger->contactLinks.equal_range(bodyItemRobot()->body()->link(i)->name() );
			for (multimap<string, string>::iterator it2 = ppp.first; it2 != ppp.second; ++it2){
				if(it2->second == bodyItemRobot()->body()->link(j)->name()) pass = true;
			}
			ppp = targetArmFinger->contactLinks.equal_range(bodyItemRobot()->body()->link(j)->name() );
			for (multimap<string, string>::iterator it2 = ppp.first; it2 != ppp.second; ++it2){
				if(it2->second == bodyItemRobot()->body()->link(i)->name()) pass = true;
			}
			if(pass) continue;
#ifdef  CNOID_10_11_12_13
			ColdetLinkPairPtr temp= new ColdetLinkPair(bodyItemRobot()->body()->link(i),bodyItemRobot()->body()->link(j));
#else
			ColdetLinkPairPtr temp = make_shared<ColdetLinkPair>(bodyItemRobot()->body(), bodyItemRobot()->body()->link(i), bodyItemRobot()->body(), bodyItemRobot()->body()->link(j) );
#endif
			temp->updatePositions();
			int t1,t2;
			double p1[3],p2[3];
			double distance = temp->computeDistance(t1,p1,t2,p2);
			if(distance>1.0e-04)	robotSelfPairs.push_back(temp);
#ifdef DEBUG_MODE
			else os <<"collide on initial condition at robotSelfPair"  <<distance <<" "<< temp->model(0)->name() <<" " << temp->model(1)->name()  << endl;
#endif
		}
	}
/*	for(unsigned int i=0; i<arm()->arm_path->numJoints(); i++){
		for(unsigned int j=i+2; j<arm()->arm_path->numJoints(); j++){
			robotSelfPairs.push_back(new ColdetLinkPair(arm()->arm_path->joint(i), arm()->arm_path->joint(j)) );
		}
	}

*/
	initialCollision();
	return true;
}

bool PlanBase::flush(){
	static int cnt=0;
	cnt++;

	if(stopFlag){
		stopFlag=false;
		throw(cnt);
	}
/* it will be GraspController
	if(bodyItemGRC){
		bodyItemGRC->body()->link(0)->R = palm()->R*(GRCmax.R);
		bodyItemGRC->body()->link(0)->p = palm()->p+palm()->R*GRCmax.p;
		bodyItemGRC->notifyKinematicStateChange();
	}
*/
//	bodyItemRobot()->body()->calcForwardKinematics();
	if(targetArmFinger) bodyItemRobot()->notifyKinematicStateChange();
	if(targetObject) targetObject->bodyItemObject->notifyKinematicStateChange();
	MessageView::mainInstance()->flush();

#ifdef  DEBUG_MODE
	usleep(100000);
#endif
	return true;

}

void PlanBase::calcForwardKinematics(){

	setInterLink();

	bodyItemRobot()->body()->calcForwardKinematics();

	if(graspingState==GRASPING) {
		if(nFing()>0){
            object()->R() = fingers(0)->tip->R()*(targetArmFinger->objectPalmRot);
            object()->p() = fingers(0)->tip->p()+fingers(0)->tip->R()*targetArmFinger->objectPalmPos;
		}else{
			object()->R() = palm()->R()*(targetArmFinger->objectPalmRot);
			object()->p() = palm()->p()+palm()->R()*targetArmFinger->objectPalmPos;
		}
	}
	else if(graspingState2==GRASPING) {
		if(nFing(1)>0) {
            object()->R() = fingers(1,0)->tip->R()*(armsList[1]->objectPalmRot);
			object()->p() = fingers(1,0)->tip->p()+fingers(1,0)->tip->R()*armsList[1]->objectPalmPos;
		} else {
			object()->R() = palm(1)->R()*(armsList[1]->objectPalmRot);
			object()->p() = palm(1)->p()+palm(1)->R()*armsList[1]->objectPalmPos;
		}
	}
	for(int i=0;i<interObjectList.size();i++){
//		if(interObjectList[i].type == InterObject::GRASPED_OBJECT){
			interObjectList[i].setInterObject();
//		}
	}
	//cout << "pos y "<< bodyItemRobot()->body()->link(0)->p.transpose() <<  rpyFromRot(bodyItemRobot()->body()->link(0)->R)[2] << endl;
}

bool PlanBase::isColliding(){
//	cnoid::ColdetLinkPairPtr* robotSelfPairs, robotEnvPairs, robotObjPairs, objEnvPairs;
	for(int i=0;i<robotSelfPairs.size();i++){
		ColdetLinkPairPtr testPair = robotSelfPairs[i];
		testPair->updatePositions();
		bool coll = testPair->checkCollision();
		if(coll){
			colPairName[0] = testPair->model(0)->name();
			colPairName[1] = testPair->model(1)->name();
#ifdef DEBUG_MODE
			cout <<"self collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
			return true;
		}
	}
	int sizeRobotEnv = robotEnvPairs.size();
	if(useRobotSafeBoundingBox) sizeRobotEnv = safeRobotEnvPairs.size();
	for(int i=0;i<sizeRobotEnv;i++){
		ColdetLinkPairPtr testPair;
		if(useRobotSafeBoundingBox) testPair= safeRobotEnvPairs[i];
		else testPair= robotEnvPairs[i];

		testPair->updatePositions();
		bool coll = testPair->checkCollision();
		if(coll){
			colPairName[0] = testPair->model(0)->name();
			colPairName[1] = testPair->model(1)->name();
#ifdef DEBUG_MODE
			cout <<"robot env collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
			return true;
		}
	}
	for(int i=0;i<robotPointCloudPairs.size();i++){
		bool coll = isCollidingPointCloud(robotPointCloudPairs[i].second,robotPointCloudPairs[i].first);
		if(coll){
#ifdef DEBUG_MODE
			cout << "robot pointcloud collide" << endl;
#endif
			return true;
		}
	}
	if(checkAllGraspingState()==NOT_GRASPING){
		for(int i=0;i<robotObjPairs.size();i++){
			ColdetLinkPairPtr testPair = robotObjPairs[i];
			testPair->updatePositions();
			bool coll = testPair->checkCollision();
			if(coll){
				colPairName[0] = testPair->model(0)->name();
				colPairName[1] = testPair->model(1)->name();
#ifdef DEBUG_MODE
				cout <<"robot obj collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
				return true;
			}
		}
	}
	if(getObjectContactState()==OFF_ENVIRONMENT){
		for(int i=0;i<objEnvPairs.size();i++){
			ColdetLinkPairPtr testPair = objEnvPairs[i];
			testPair->updatePositions();
			bool coll = testPair->checkCollision();
			if(coll){
				colPairName[0] = testPair->model(0)->name();
				colPairName[1] = testPair->model(1)->name();
#ifdef DEBUG_MODE
				cout <<"obj env collide " << i  << " "<<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
				return true;
			}
		}
		for(int i=0;i<objPointCloudPairs.size();i++){
			bool coll = isCollidingPointCloud(objPointCloudPairs[i].second ,objPointCloudPairs[i].first);
			if(coll){
#ifdef DEBUG_MODE
				cout << "obj pointcloud collide" << endl;
#endif
				return true;
			}
		}
	}

	for(int i=0;i<interObjectList.size();i++){
		if( interObjectList[i].isColliding() ) return true;
	}

	return false;
}

bool PlanBase::isCollidingPointCloud(PointCloudEnv* pc_env, BodyItemPtr item, double tol){
	for(int i=0; i<item->body()->numLinks(); i++) {
		ColdetModelPtr c = item->body()->link(i)->coldetModel();
		vector<Vector3> bbdata;
		c->getBoundingBoxData(0, bbdata);
		vector<Vector3> v(8);
		v[0] = bbdata[0] + Vector3( bbdata[1](0),  bbdata[1](1),  bbdata[1](2));
		v[1] = bbdata[0] + Vector3( bbdata[1](0),  bbdata[1](1), -bbdata[1](2));
		v[2] = bbdata[0] + Vector3( bbdata[1](0), -bbdata[1](1),  bbdata[1](2));
		v[3] = bbdata[0] + Vector3( bbdata[1](0), -bbdata[1](1), -bbdata[1](2));
		v[4] = bbdata[0] + Vector3(-bbdata[1](0),  bbdata[1](1),  bbdata[1](2));
		v[5] = bbdata[0] + Vector3(-bbdata[1](0),  bbdata[1](1), -bbdata[1](2));
		v[6] = bbdata[0] + Vector3(-bbdata[1](0), -bbdata[1](1),  bbdata[1](2));
		v[7] = bbdata[0] + Vector3(-bbdata[1](0), -bbdata[1](1), -bbdata[1](2));

		double max_x, max_y, max_z, min_x, min_y, min_z;
		max_x = max_y = max_z = -DBL_MAX;
		min_x = min_y = min_z = DBL_MAX;
		for (int j = 0; j < 8; j++) {
			Vector3 rv;
            rv = item->body()->link(i)->R() * v[j] + item->body()->link(i)->p();
			max_x = std::max(max_x, rv(0));
			min_x = std::min(min_x, rv(0));
			max_y = std::max(max_y, rv(1));
			min_y = std::min(min_y, rv(1));
			max_z = std::max(max_z, rv(2));
			min_z = std::min(min_z, rv(2));
		}

		max_x += tol; max_y += tol; max_z += tol;
		min_x -= tol; min_y -= tol; min_z -= tol;

		if(max_x < pc_env->min_x || min_x > pc_env->max_x ||
			max_y < pc_env->min_y || min_y > pc_env->max_y ||
			max_z < pc_env->min_z || min_z > pc_env->max_z) {
				continue;
		}

		vector<Vector3> target_p;
		for(int j=0;j<pc_env->p().size();j++){
			if(pc_env->p()[j](0) < max_x && pc_env->p()[j](0) > min_x &&
				pc_env->p()[j](1) < max_y && pc_env->p()[j](1) > min_y &&
				pc_env->p()[j](2) < max_z && pc_env->p()[j](2) > min_z){
					target_p.push_back(pc_env->p()[j]);
			}
		}

		if(target_p.empty()) continue;

		if(item->body()->link(i)->coldetModel()->checkCollisionWithPointCloud(target_p, tol)){
			return true;
		}
	}
	return false;
}
/* //オリジナル
bool PlanBase::isCollidingPointCloud(const vector<Vector3>& p, BodyItemPtr item, double tol){
	for(int i=0; i<item->body()->numLinks(); i++){
			if(item->body()->link(i)->coldetModel()->checkCollisionWithPointCloud(p, tol)){
					return true;
			}
	}
	return false;
}
*/
//①一点Ver
int PlanBase::isCollidingPointCloud(const vector<Vector3>& p, BodyItemPtr item, double tol){
  //sleep(2); //test
  for(int i=0; i<item->body()->numLinks(); i++){
    int judge = item->body()->link(i)->coldetModel()->checkCollisionWithPointCloud(p, tol);
    if(judge) {
        return judge;
    }
  }
  return 0;
}
//入力ベクタpの干渉点をcol_resultに詰める
void PlanBase::isCollidingPointCloud2(const vector<cnoid::Vector3>& p, cnoid::BodyItemPtr item, vector<cnoid::Vector3>& col_result, double tol){
  for(int i=0; i<item->body()->numLinks(); i++){
    item->body()->link(i)->coldetModel()->checkCollisionWithPointCloud2(p, tol, col_result);
  }
}
//2の重複を消したもの
void PlanBase::isCollidingPointCloud3(const vector<cnoid::Vector3>& p, cnoid::BodyItemPtr item, vector<cnoid::Vector3>& col_result, double tol){
  vector<int> judge;
  for(int i=0; i<p.size(); i++){
    judge.push_back(0);
  }
  //for(int i=0; i<item->body()->numLinks(); i++){
  for(int i=3; i<11; i++){ //for pal Right Arm
    item->body()->link(i)->coldetModel()->checkCollisionWithPointCloud3(p, tol, judge);
  }
  for(int i=0; i<p.size(); i++){
    if(judge[i]){
      col_result.push_back(p[i]);
    }
  }
}

//入力ベクタpから干渉点を削除する
void PlanBase::removeCollidingPointCloud(vector<cnoid::Vector3>& p, cnoid::BodyItemPtr item, double tol){
  for(int i=0; i<item->body()->numLinks(); i++){
    item->body()->link(i)->coldetModel()->removeCollisionWithPointCloud(p, tol);
  }
}
void PlanBase::removeCollidingPointCloud2(const vector<cnoid::Vector3>& p, cnoid::BodyItemPtr item, vector<cnoid::Vector3>& col_result, double tol){
  vector<int> judge;
  for(int i=0; i<p.size(); i++){
    judge.push_back(0);
  }
  for(int i=0; i<item->body()->numLinks(); i++){
    item->body()->link(i)->coldetModel()->removeCollisionWithPointCloud2(p, tol, judge);
  }
  for(int i=0; i<p.size(); i++){
    if(!judge[i]){
      col_result.push_back(p[i]);
    }
  }
}

bool PlanBase::isCollidingPointCloudFinger(const vector<Vector3>& p, double tol){

	for(int i=0; i<nFing(); i++)
		for(int j=0; j<fingers(i)->fing_path->numJoints(); j++){
			if(fingers(i)->fing_path->joint(j)->coldetModel()->checkCollisionWithPointCloud(p, tol))
					return true;
		}
	return false;
}

double PlanBase::clearance(){

//	double start = getrusage_sec();

	double min_sep=1.e10;

	for(int i=0;i<robotSelfPairs.size();i++){
		ColdetLinkPairPtr testPair = robotSelfPairs[i];
		testPair->updatePositions();
		bool coll = testPair->checkCollision();
		if(coll){
			colPairName[0] = testPair->model(0)->name();
			colPairName[1] = testPair->model(1)->name();
#ifdef DEBUG_MODE
			cout <<"self collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
			return 0;
		}
	}
	if(checkAllGraspingState()==NOT_GRASPING){
		for(int i=0;i<robotObjPairs.size();i++){
			ColdetLinkPairPtr testPair = robotObjPairs[i];
			testPair->updatePositions();
			bool coll = testPair->checkCollision();
			if(coll){
				colPairName[0] = testPair->model(0)->name();
				colPairName[1] = testPair->model(1)->name();
#ifdef DEBUG_MODE
				cout <<"robot obj collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
				return 0;
			}
		}
	}

	for(int i=0;i<robotPointCloudPairs.size();i++){
		bool coll = isCollidingPointCloud(robotPointCloudPairs[i].second,robotPointCloudPairs[i].first);
		if(coll){
#ifdef DEBUG_MODE
			cout << "robot pointcloud collide" << endl;
#endif
			return 0;
		}
	}

	for(int i=0;i<robotEnvPairs.size();i++){
		ColdetLinkPairPtr testPair = robotEnvPairs[i];
		testPair->updatePositions();

		testPair->setTolerance(tolerance);
		if( testPair->detectIntersection() ){
#ifdef DEBUG_MODE
			os <<"rob-env tolerance collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
			return 0;
		}
		continue;

		int t1,t2;
		double p1[3],p2[3];
		double distance = testPair->computeDistance(t1,p1,t2,p2);
		if(distance <=tolerance){
#ifdef DEBUG_MODE
			os <<"rob-env tolerance collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
			return distance;
		}
		else {
			os << distance << endl;
		}
		if(distance < min_sep){
			min_sep = distance;
		}
	}

	if(getObjectContactState()==OFF_ENVIRONMENT){
		for(int i=0;i<objEnvPairs.size();i++){
			ColdetLinkPairPtr testPair = objEnvPairs[i];
			testPair->updatePositions();

			testPair->setTolerance(tolerance);
			if( testPair->detectIntersection() ){
#ifdef DEBUG_MODE
				os <<"rob-env tolerance collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
				return 0;
			}
			continue;

			int t1,t2;
			double p1[3],p2[3];
			double distance = testPair->computeDistance(t1,p1,t2,p2);
			if(distance <=tolerance){
#ifdef DEBUG_MODE
				os <<"obj-env tolerance collide " <<testPair->model(0)->name() <<" "<<testPair->model(1)->name()<< endl;
#endif
				return distance;
			}
			else {
				os << distance << endl;
			}
			if(distance < min_sep){
				min_sep = distance;
			}
		}

		for(int i=0;i<objPointCloudPairs.size();i++){
			bool coll = isCollidingPointCloud(objPointCloudPairs[i].second, objPointCloudPairs[i].first);
			if(coll){
	#ifdef DEBUG_MODE
				cout << "obj pointcloud collide" << endl;
	#endif
				return 0;
			}
		}
	}
//	double end = getrusage_sec();
//	cout << "time clearance" << objEnvPairs.size() << " "<< end - start << endl;

	return min_sep;
}

void PlanBase::setGraspingState(int state){
	if(state==GRASPING){
		if(nFing() > 0){
			targetArmFinger->objectPalmPos = trans(Matrix3(fingers(0)->tip->R()))*(object()->p() - fingers(0)->tip->p());
			targetArmFinger->objectPalmRot = trans(Matrix3(fingers(0)->tip->R()))*object()->R();
		}else{
			targetArmFinger->objectPalmPos = trans(Matrix3(palm()->R()))*(object()->p() - palm()->p());
			targetArmFinger->objectPalmRot = trans(Matrix3(palm()->R()))*object()->R() ;
		}
	}
	graspingState = state;
}

void PlanBase::setGraspingState2(int state){
	if(armsList.size() >1 && state==GRASPING){
		if(nFing(1)>0) {
			armsList[1]->objectPalmPos = trans(Matrix3(fingers(1,0)->tip->R()))*(object()->p() - fingers(1,0)->tip->p());
			armsList[1]->objectPalmRot = trans(Matrix3(fingers(1,0)->tip->R()))*object()->R();
		}else {
			armsList[1]->objectPalmPos = trans(Matrix3(palm(1)->R()))*(object()->p() - palm(1)->p());
			armsList[1]->objectPalmRot = trans(Matrix3(palm(1)->R()))*object()->R() ;
		}
	}
	graspingState2 = state;
}

void PlanBase::setTrajectoryPlanDOF(){

	pathPlanDOF.clear();

	for(int i=0; i<arm()->nJoints; i++)
        pathPlanDOF.push_back(arm()->arm_path->joint(i)->jointId());
	for(int i=0; i<nFing(); i++)
		for(int j=0; j<fingers(i)->nJoints; j++)
            pathPlanDOF.push_back(fingers(i)->fing_path->joint(j)->jointId());

#ifdef DEBUG_MODE
	cout << "Plan DOF= ";
	for (unsigned int i=0; i<pathPlanDOF.size(); i++)
		cout << pathPlanDOF[i] << " ";	cout << endl;
#endif
}

void PlanBase::setTrajectoryPlanDOF(int k){

	pathPlanDOF.clear();

	for(int i=0; i<arm(k)->nJoints; i++)
        pathPlanDOF.push_back(arm(k)->arm_path->joint(i)->jointId());
	for(int i=0; i<nFing(k); i++)
		for(int j=0; j<fingers(k,i)->nJoints; j++)
            pathPlanDOF.push_back(fingers(k,i)->fing_path->joint(j)->jointId());

#ifdef DEBUG_MODE
	cout << "Plan DOF= ";
	for (unsigned int i=0; i<pathPlanDOF.size(); i++)
		cout << pathPlanDOF[i] << " ";	cout << endl;
#endif
}

void PlanBase::setTrajectoryPlanMapDOF(){

	pathPlanDOF.clear();

	int top = body()->numJoints();
	pathPlanDOF.push_back(top); //position x
	pathPlanDOF.push_back(top+1); //position y;
	pathPlanDOF.push_back(top+5); //yaw;

	//ulimitMap = Vector3(3.0,3.0,3.0);
	//llimitMap = Vector3(-3.0,-3.0,-3.0);

#ifdef DEBUG_MODE
	cout << "Plan Map DOF= ";
	for (unsigned int i=0; i<pathPlanDOF.size(); i++)
		cout << pathPlanDOF[i] << " ";	cout << endl;
#endif
}

double PlanBase::calcContactPoint(ColdetLinkPairPtr cPair, Vector3 &Po, Vector3 &Pf, Vector3 &objN, Vector3 &fingerN) {

//	int Ik = t;

	double p1[3] = {0}, p2[3] = {0};
	int tid1, tid2;

//	ColdetLinkPairPtr cPair = linkObjPair[Ik];
//	ColdetModelPtr model = cPair->model(0);

    cPair->model(0)->setPosition(cPair->link(0)->R(), cPair->link(0)->p());
    cPair->model(1)->setPosition(cPair->link(1)->R(), cPair->link(1)->p());
//	cout << cPair->link(0)->p << cPair->link(1)->p << endl;

	double dsn = cPair->computeDistance(tid1, &p1[0], tid2, &p2[0]);

	Link* links[2];
	links[0] = cPair->link(0);
	links[1] = cPair->link(1);

	int v[2][3];
	links[0]->coldetModel()->getTriangle(tid1, v[0][0], v[0][1], v[0][2]);
	links[1]->coldetModel()->getTriangle(tid2, v[1][0], v[1][1], v[1][2]);

	float p[3];
	Vector3 n[3];

	for (int i = 1; i < 2;i++) {
		for (int j = 0; j < 3;j++) {
			links[i]->coldetModel()->getVertex(v[i][j], p[0], p[1], p[2]);
			n[j] = Vector3 (p[0], p[1], p[2]);
		}
	}

	Pf = Vector3(p1[0], p1[1], p1[2]);
	Po = Vector3(p2[0], p2[1], p2[2]);
//	cout << Po << Pf << endl;

	//Po = trans(cPair->link(1)->R) * Po - cPair->link(1)->p; //bug? ochi
    Po = trans(Matrix3(cPair->link(1)->R())) * (Po - cPair->link(1)->p());
//	alias(Pf) = trans(cPair->link(0)->R) * Pf - cPair->link(0)->p;

	Vector3 objN2 = cross(Vector3(n[1] - n[0]), Vector3(n[2] - n[0]));
	objN = objN2 / norm2(objN2);

	for (int j = 0; j < 3;j++) {
		links[0]->coldetModel()->getVertex(v[0][j], p[0], p[1], p[2]);
		n[j] = Vector3 (p[0], p[1], p[2]);
	}

	objN2 = cross(Vector3(n[1] - n[0]), Vector3(n[2] - n[0]));
	//normal vector of finger in the object local corrdinate
    fingerN = trans(Matrix3(cPair->link(1)->R())) * (cPair->link(0)->R()) * (objN2 / norm2(objN2));

	return dsn;

}

Matrix3 makeOrthogonal(const MatrixXd A, const VectorXd b)
{
	vector<double> c;
	for(size_t i=0; i<b.size(); i++)
			c.push_back(fabs(b(i)));

	Matrix3 A_ = d2v(A);
	int j=argmax(c);
	Vector3 v0 = A_.col(j);
	Vector3 v1 = A_.col((j+1)%3);

	double r=dot(v0,v1);
	v1 = (-r*v0 + v1)/sqrt(1-r*r);
	Vector3 v2 = cross(v0,v1);

	for(int i=0; i<3; i++){
		A_(i, (j+1)%3) = v1(i);
		A_(i, (j+2)%3) = v2(i);
	}

	return A_;
}

void PlanBase::calcBoundingBox(ColdetModelPtr model, Vector3 &edge, Vector3& center, Vector3& com, Matrix3& Rot) {
	//objVis and objPos shoulde be defined in advance.

	class Triangle{
	public:
		cnoid::Vector3 ver[3];
		float area;
	};

	// convert coldetmodel to objectshape
	float out_x, out_y, out_z;
	int v0,v1,v2;

	int nVerticies = model->getNumVertices();
	int nTriangles = model->getNumTriangles();

	Vector3* verticies = new Vector3[nVerticies];
	Triangle* triangles = new Triangle[nTriangles];

	for(int i=0;i<nVerticies;i++){
        model->getVertex(i, out_x, out_y, out_z);
		verticies[i][0] = out_x;
		verticies[i][1] = out_y;
		verticies[i][2] = out_z;
	}

	for(int i=0;i<nTriangles;i++){
        model->getTriangle(i, v0,v1,v2);
		triangles[i].ver[0] = verticies[v0];
		triangles[i].ver[1] = verticies[v1];
		triangles[i].ver[2] = verticies[v2];
	}

	// calc distribution
	Vector3 pt;
	MatrixXd distribute = MatrixXd::Zero(3, 3);
	Vector3 average(0, 0, 0);

	for(int i=0;i<nTriangles;i++){
		Vector3 e1 (triangles[i].ver[1] - triangles[i].ver[0]);
		Vector3 e2 (triangles[i].ver[2] - triangles[i].ver[0]);
		triangles[i].area = norm2 ( cross(e1,e2) ) /2.0;
	}

	Matrix3 aq,aq_n_sum,aq_p_sum;
	std::vector<double>* aq_p[3][3];
	std::vector<double>* aq_n[3][3];
	Vector3 sumCenter(0,0,0);
	double sumArea=0;
	for(int i=0;i<3;i++) for(int j=0;j<3;j++) {
		aq(i,j)=0;
		aq_p_sum(i,j)=0;
		aq_n_sum(i,j)=0;
		aq_p[i][j] = new std::vector<double>();
		aq_n[i][j] = new std::vector<double>();
	}

	for(int l=0;l<nTriangles;l++){
		Triangle& t = triangles[l];
		for(int i=0;i<3;i++){
			for(int j=0;j<3;j++){
				for(int k=0;k<3;k++){
					double tmp_aq;
					tmp_aq = t.area*(t.ver[i][j] * t.ver[i][k])/6.0;
					tmp_aq > 0 ? aq_p[j][k]->push_back(tmp_aq) : aq_n[j][k]->push_back(tmp_aq);
					tmp_aq = t.area*(t.ver[i][j] * t.ver[(i+1)%3][k])/12.0;
					tmp_aq > 0 ? aq_p[j][k]->push_back(tmp_aq) : aq_n[j][k]->push_back(tmp_aq);
					tmp_aq = t.area*(t.ver[(i+1)%3][j] * t.ver[i][k])/12.0;
					tmp_aq > 0 ? aq_p[j][k]->push_back(tmp_aq) : aq_n[j][k]->push_back(tmp_aq);
				}
			}
		}
		sumArea +=t.area;
		sumCenter  =  sumCenter + t.area/3.0* Vector3 ( t.ver[0] + t.ver[1] + t.ver[2]);
	}
	for(int i=0;i<3;i++) for(int j=0;j<3;j++) {
		std::sort(aq_p[i][j]->begin(),aq_p[i][j]->end(),LessAbs());
		for(int n=0;n<aq_p[i][j]->size();n++){
			aq_p_sum(i,j) += aq_p[i][j]->at(n);
		}
		std::sort(aq_n[i][j]->begin(),aq_n[i][j]->end(),LessAbs());
		for(int n=0;n<aq_n[i][j]->size();n++){
			aq_n_sum(i,j) += aq_n[i][j]->at(n);
		}
		delete aq_p[i][j];
		delete aq_n[i][j];
	}
	for(int i=0;i<3;i++) for(int j=0;j<3;j++) {aq(i,j) = aq_p_sum(i,j) + aq_n_sum(i,j);}
	average = com = sumCenter/sumArea;
	for(int j=0;j<3;j++){
		for(int k=0;k<3;k++){
			distribute(j,k) = aq(j,k) - com[j] * com[k] * sumArea;
		}
	}
	MatrixXd evec(3, 3);
	VectorXd eval(3);
	int info;
	Eigen::EigenSolver<MatrixXd> es(distribute);
	eval = es.eigenvalues().real();
	evec = es.eigenvectors().real();

	Rot = makeOrthogonal(evec, eval);
	//Rot =  (d2v(evec));

	Vector3 e[3];
	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			e[j][i] = Rot(i, j);

	Vector3 pt_max(0, 0, 0), pt_min(0, 0, 0);

	for(int l=0;l<nVerticies;l++){
		Vector3 pt = verticies[l];
		for (int j = 0; j < 3; j++) {
			double tmp = dot(e[j], Vector3(pt - average));
			if (tmp > pt_max[j]) pt_max[j] = tmp;
			if (tmp < pt_min[j]) pt_min[j] = tmp;
		}
	}

	//Rot =  (d2v(evec));

	edge =  (pt_max - pt_min);
	center =  average + 0.5 * Rot * (pt_max + pt_min);
//	com = average;

//	alias(Rot)  = objVisRot() * Rot;
//	alias(center) = objVisRot() * center + objVisPos();
//	alias(com)    = objVisRot() * com   + objVisPos();
#ifdef DEBUG_MODE
	cout << "bouding box size"<< edge.transpose() << endl<< Rot  <<endl;
#endif

	delete []	verticies;
	delete []  triangles;

	return;
}

//Including Enveloping grasp
//bool PlanBase::sampleFinalPos2(mpkRobotCollection* robots, vector<mpkCollPair> *test_pairs, int iterate)
bool PlanBase::initial() {
	if ( !targetObject || !targetArmFinger) {
		os << "Please select Grasped Object and Grasping Robot" << endl;
		return false;
	}

	RemoveEnvironment(targetObject->bodyItemObject);

    targetObject->objVisPos = object()->p();
    targetObject->objVisRot = object()->R();

	setGraspingState(NOT_GRASPING);
	setGraspingState2(NOT_GRASPING);
	graspMotionSeq.push_back ( getMotionState() );

	initialCollision();

	_initrand();
	return true;
}

void PlanBase::initialCollision(){

	/////////////////////////////////////////////////////////// temporal change
	initialCollisionWithMemory();
	return;
	///////////////////////////////////////////////////////////

	if(!doInitialCollision) return;

	robotEnvPairs.clear();
	robotObjPairs.clear();
	objEnvPairs.clear();
	robotPointCloudPairs.clear();
	objPointCloudPairs.clear();

	if(targetArmFinger==NULL) {
		return;
	}
	for(unsigned int j=0;j<bodyItemRobot()->body()->numLinks();j++){
		for( list<BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end(); it++){
			for(unsigned int i=0;i<(*it)->body()->numLinks();i++){
#ifdef  CNOID_10_11_12_13
				ColdetLinkPairPtr temp= new ColdetLinkPair(bodyItemRobot()->body()->link(j), (*it)->body()->link(i));
#else
				ColdetLinkPairPtr temp = make_shared<ColdetLinkPair>(bodyItemRobot()->body(),bodyItemRobot()->body()->link(j), (*it)->body(), (*it)->body()->link(i) );
#endif
				temp->updatePositions();
				int t1,t2;
				double p1[3],p2[3];
				double distance = temp->computeDistance(t1,p1,t2,p2);
				if(distance>1.0e-04)	robotEnvPairs.push_back(temp);
#ifdef DEBUG_MODE
				else os <<"collide on initial condition robot and env"  <<distance <<" "<< temp->model(0)->name() <<" " << (*it)->body()->name() << endl;
#endif
			}
		}
	}
/*	for(unsigned int j=0;j<arm()->arm_path->numJoints();j++){
		for( list<BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end(); it++){
			ColdetLinkPairPtr temp= new ColdetLinkPair(arm()->arm_path->joint(j), (*it)->body()->link(0));
			temp->model(0)->setPosition(temp->link(0)->R, temp->link(0)->p);
			temp->model(1)->setPosition(temp->link(1)->R, temp->link(1)->p);
			int t1,t2;
			double p1[3],p2[3];
			double distance = temp->computeDistance(t1,p1,t2,p2);
			if(distance>1.0e-03)	armEnvPairs.push_back(temp);
#ifdef DEBUG_MODE
			else os <<"tollerance collide on initial condition"  <<distance <<" "<< temp->model(0)->name() <<" " << temp->model(1)->name()  << endl;
#endif
			os <<"tollerance collide on initial condition"  <<distance <<" "<< temp->model(0)->name() <<" " << temp->model(1)->name()  << endl;
		}
	}
*/	if(targetObject){
		for(unsigned int j=0;j<bodyItemRobot()->body()->numLinks();j++){
#ifdef  CNOID_10_11_12_13
				robotObjPairs.push_back(new ColdetLinkPair(bodyItemRobot()->body()->link(j), object() ));
#else
				robotObjPairs.push_back(make_shared<ColdetLinkPair>(bodyItemRobot()->body(),bodyItemRobot()->body()->link(j),targetObject->bodyItemObject->body(),object()));
#endif

			}
		ColdetModelPtr backup = object()->coldetModel();
		if(useObjectSafeBoundingBox){
			object()->setColdetModel(targetObject->safeBoundingBox);
		}
		for(list<cnoid::BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end();it++){
			for(unsigned int i=0;i<(*it)->body()->numLinks();i++){
#ifdef  CNOID_10_11_12_13
				objEnvPairs.push_back(new ColdetLinkPair(object(), (*it)->body()->link(i)));
#else
				objEnvPairs.push_back( make_shared<ColdetLinkPair>(targetObject->bodyItemObject->body(), object(), (*it)->body(), (*it)->body()->link(i)));
#endif
			}
        }
        object()->setColdetModel(backup);
	}

	for(list<PointCloudEnv*>::iterator it = pointCloudEnv.begin(); it != pointCloudEnv.end(); ++it){
		pair<BodyItemPtr, PointCloudEnv*> robo_pointcloud(bodyItemRobot(), *it);
		robotPointCloudPairs.push_back(robo_pointcloud);
		if(targetObject){
			pair<BodyItemPtr, PointCloudEnv*> obj_pointcloud(targetObject->bodyItemObject, *it);
			objPointCloudPairs.push_back(obj_pointcloud);
		}
	}

	for(int i=0;i<interObjectList.size();i++) interObjectList[i].initialCollision();
}

void PlanBase::initialCollisionWithMemory(){

//	static vector<ColdetPairData*> coldetPairData;

	robotEnvPairs.clear();
	safeRobotEnvPairs.clear();
	robotObjPairs.clear();
	objEnvPairs.clear();
	robotPointCloudPairs.clear();
	objPointCloudPairs.clear();

	if(targetArmFinger) {
		for( list<BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end(); it++){
			BodyItemPtr bodyItem1= bodyItemRobot();
			BodyItemPtr bodyItem2= *it;
			bool find=false;
			for(int k=0;k<coldetPairData.size();k++){
				if( ( (coldetPairData[k]->bodyItem1== bodyItem1) && (coldetPairData[k]->bodyItem2== bodyItem2) ) ||
				       ((coldetPairData[k]->bodyItem2== bodyItem2) && (coldetPairData[k]->bodyItem1== bodyItem1) ) ){
					find=true;
					robotEnvPairs.insert( robotEnvPairs.end(),  coldetPairData[k]->coldetLinkPairs.begin(), coldetPairData[k]->coldetLinkPairs.end() );
					break;
				}

			}
			if(find) continue;
			ColdetPairData * temp = new ColdetPairData(bodyItem1, bodyItem2);
			coldetPairData.push_back(temp);
			robotEnvPairs.insert( robotEnvPairs.end(),  temp->coldetLinkPairs.begin(), temp->coldetLinkPairs.end() );
		}

		for( list<BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end(); it++){
			BodyItemPtr bodyItem1= bodyItemRobot();
			BodyItemPtr bodyItem2= *it;
			bool find=false;
			for(int k=0;k<safeColdetPairData.size();k++){
				if( ( (safeColdetPairData[k]->bodyItem1== bodyItem1) && (safeColdetPairData[k]->bodyItem2== bodyItem2) ) ||
				       ((safeColdetPairData[k]->bodyItem2== bodyItem2) && (safeColdetPairData[k]->bodyItem1== bodyItem1) ) ){
					find=true;
					safeRobotEnvPairs.insert( safeRobotEnvPairs.end(),  safeColdetPairData[k]->coldetLinkPairs.begin(), safeColdetPairData[k]->coldetLinkPairs.end() );
					break;
				}

			}
			if(find) continue;
			ColdetPairData * temp = new ColdetPairData(bodyItem1, bodyItem2, false, true);
			safeColdetPairData.push_back(temp);
			safeRobotEnvPairs.insert( safeRobotEnvPairs.end(),  temp->coldetLinkPairs.begin(), temp->coldetLinkPairs.end() );
		}

	}

	if(targetObject){
		if(targetArmFinger) {
			BodyItemPtr bodyItem1= bodyItemRobot();
			BodyItemPtr bodyItem2= targetObject->bodyItemObject;
			bool find=false;
			for(int k=0;k<coldetPairData.size();k++){
				if( ( (coldetPairData[k]->bodyItem1== bodyItem1) && (coldetPairData[k]->bodyItem2== bodyItem2) ) ||
				       ((coldetPairData[k]->bodyItem2== bodyItem2) && (coldetPairData[k]->bodyItem1== bodyItem1) ) ){
					find=true;
					robotObjPairs.insert( robotObjPairs.end(),  coldetPairData[k]->coldetLinkPairs.begin(), coldetPairData[k]->coldetLinkPairs.end() );
					break;
				}

			}
			if(!find){
				ColdetPairData * temp = new ColdetPairData(bodyItem1, bodyItem2);
				coldetPairData.push_back(temp);
				robotObjPairs.insert( robotObjPairs.end(),  temp->coldetLinkPairs.begin(), temp->coldetLinkPairs.end() );
			}
		}

		for(list<cnoid::BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end();it++){
			BodyItemPtr bodyItem1= targetObject->bodyItemObject;
			BodyItemPtr bodyItem2= *it;
			bool find=false;
			for(int k=0;k<coldetPairData.size();k++){
				if( ( (coldetPairData[k]->bodyItem1== bodyItem1) && (coldetPairData[k]->bodyItem2== bodyItem2) ) ||
				       ((coldetPairData[k]->bodyItem2== bodyItem2) && (coldetPairData[k]->bodyItem1== bodyItem1) ) ){
					find=true;
					objEnvPairs.insert( objEnvPairs.end(),  coldetPairData[k]->coldetLinkPairs.begin(), coldetPairData[k]->coldetLinkPairs.end() );
					break;
				}
			}
			if(find) continue;
			ColdetPairData * temp = new ColdetPairData(bodyItem1, bodyItem2);
			coldetPairData.push_back(temp);
			objEnvPairs.insert( objEnvPairs.end(),  temp->coldetLinkPairs.begin(), temp->coldetLinkPairs.end() );
		}
		if(useObjectSafeBoundingBox){
			objEnvPairs.clear();
            ColdetModelPtr backup = object()->coldetModel();
            object()->setColdetModel(targetObject->safeBoundingBox);
			for(list<cnoid::BodyItemPtr>::iterator it = bodyItemEnv.begin(); it !=bodyItemEnv.end();it++){
				for(unsigned int i=0;i<(*it)->body()->numLinks();i++){
#ifdef  CNOID_10_11_12_13
					objEnvPairs.push_back(new ColdetLinkPair(object(), (*it)->body()->link(i)));
#else
					objEnvPairs.push_back( make_shared<ColdetLinkPair>(targetObject->bodyItemObject->body(), object(), (*it)->body(), (*it)->body()->link(i)));
#endif
				}
			}
//			object()->coldetModel() = backup;
			object()->setColdetModel(backup);
		}
	}
	for(list<PointCloudEnv*>::iterator it = pointCloudEnv.begin(); it != pointCloudEnv.end(); ++it){
		if (targetArmFinger) {
			pair<BodyItemPtr, PointCloudEnv*> robo_pointcloud(bodyItemRobot(), *it);
			robotPointCloudPairs.push_back(robo_pointcloud);
		}
		if (targetObject) {
			pair<BodyItemPtr, PointCloudEnv*> obj_pointcloud(targetObject->bodyItemObject, *it);
			objPointCloudPairs.push_back(obj_pointcloud);
		}
	}
	for(int i=0;i<interObjectList.size();i++) interObjectList[i].initialCollision();

	os << "coldetPairData size" << coldetPairData.size() << endl;
}

void PlanBase::setObjPos(const cnoid::Vector3& P, const cnoid::Matrix3 R){

	targetObject->objVisPos = P;
	targetObject->objVisRot = R;
    object()->p()=P;
	object()->R()=R;

	targetObject->offsetApplied = false;

	return;

}

void PlanBase::setVisOffset(const cnoid::Vector3& P)
{
	if(!targetObject->offsetApplied){
		targetObject->objVisPos += P;
        object()->p() += P;
		targetObject->offsetApplied = true;
	}
}

void PlanBase::setVisOffsetR()
{
	Vector3 y = rpyFromRot(targetObject->objVisRot);
	if( fabs(y(0))<0.1 ) y(0)=0;
	else if(fabs(y(0)-1.5708)<0.1 ) y(0) =  1.5708;
	else if(fabs(y(0)+1.5708)<0.1 ) y(0) = -1.5708;
	else if(fabs(y(0)-3.1415)<0.1 ) y(0) =  3.1415;
	else if(fabs(y(0)+3.1415)<0.1 ) y(0) = -3.1415;

	if( fabs(y(1))<0.1 ) y(1)=0;
	else if(fabs(y(1)-1.5708)<0.1 ) y(1) =  1.5708;
	else if(fabs(y(1)+1.5708)<0.1 ) y(1) = -1.5708;
	else if(fabs(y(1)-3.1415)<0.1 ) y(1) =  3.1415;
	else if(fabs(y(1)+3.1415)<0.1 ) y(1) = -3.1415;

	targetObject->objVisRot = rotFromRpy(y);
    object()->R() = rotFromRpy(y);
}

void PlanBase::removeVisOffset(const cnoid::Vector3& P)
{
	if(targetObject->offsetApplied){
		targetObject->objVisPos -= P;
        object()->p() -= P;
		targetObject->offsetApplied = false;
	}
}

void PlanBase::setInterLink(){
	if(interLinkList.empty()) return;
	for(int i=0; i<interLinkList.size();i++){
        interLinkList[i].slave->q() = interLinkList[i].master->q() *interLinkList[i].ratio;
        if( interLinkList[i].slave->q() < interLinkList[i].slave->q_lower()) interLinkList[i].slave->q() = interLinkList[i].slave->q_lower();
        if( interLinkList[i].slave->q() > interLinkList[i].slave->q_upper()) interLinkList[i].slave->q() = interLinkList[i].slave->q_upper();
	}
}


#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <boost/functional/hash.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/collision_detection_fcl/collision_env_fcl.h>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt/collision_terms.h>
#include <trajopt/utils.hpp>
#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/expr_vec_ops.hpp>
#include <trajopt_sco/modeling_utils.hpp>
#include <trajopt_sco/sco_common.hpp>
#include <trajopt_utils/eigen_conversions.hpp>
#include <trajopt_utils/logging.hpp>
#include <trajopt_utils/stl_to_string.hpp>

namespace trajopt
{
void CollisionsToDistances(const collision_detection::DistanceMap& distance_results, sco::DblVec& dists)
{
  dists.clear();
  dists.reserve(distance_results.size());
  for (const auto& d : distance_results)
  {
    dists.push_back(d.second[0].distance);
  }
}

void CollisionsToDistanceExpressions(const collision_detection::DistanceMap& distance_results,
                                     planning_scene::PlanningSceneConstPtr env,
                                     const robot_state::JointModelGroup* joint_model_group,
                                     const sco::VarVector& vars,
                                     const sco::DblVec& dofvals,
                                     sco::AffExprVector& exprs,
                                     bool isTimestep1)
{
  exprs.clear();
  exprs.reserve(distance_results.size());
  moveit::core::RobotState robot_state(env->getRobotModel());
  robot_state.setVariablePositions(joint_model_group->getActiveJointModelNames(), dofvals);
  robot_state.update();
  for (const auto& d : distance_results)
  {
    sco::AffExpr dist(d.second[0].distance);
    Eigen::MatrixXd jacobian;
    bool robot_link1 = false, robot_link2 = false;
    if (robot_state.getJacobian(joint_model_group,
                                env->getRobotModel()->getLinkModel(d.second[0].link_names[0]),
                                d.second[0].nearest_points[0], jacobian))
    {
      Eigen::VectorXd dist_grad = -d.second[0].normal.transpose()*jacobian.topRows(3);
      sco::exprInc(dist, sco::varDot(dist_grad, vars));
      sco::exprInc(dist, -dist_grad.dot(util::toVectorXd(dofvals)));
      robot_link1 = true;
    }
    if (robot_state.getJacobian(joint_model_group,
                                env->getRobotModel()->getLinkModel(d.second[0].link_names[1]),
                                d.second[0].nearest_points[1], jacobian))
    {
      Eigen::VectorXd dist_grad = d.second[0].normal.transpose()*jacobian.topRows(3);
      sco::exprInc(dist, sco::varDot(dist_grad, vars));
      sco::exprInc(dist, -dist_grad.dot(util::toVectorXd(dofvals)));
      robot_link2 = true;
    }
    if (robot_link1 || robot_link2)
    {
      exprs.push_back(dist);
    }
  }

  LOG_DEBUG("%ld distance expressions\n", exprs.size());
}

inline size_t hash(const sco::DblVec& x){ return boost::hash_range(x.begin(), x.end()); }

void CollisionEvaluator::GetCollisionsCached(const sco::DblVec& x, collision_detection::DistanceMap& distance_results)
{
  size_t key = hash(sco::getDblVec(x, GetVars()));
  collision_detection::DistanceMap* it = m_cache.get(key);
  if (it != nullptr) {
    LOG_DEBUG("using cached collision check\n");
    distance_results = *it;
  }
  else {
    LOG_DEBUG("not using cached collision check\n");
    CalcCollisions(x, distance_results);
    m_cache.put(key, distance_results);
  }
}

SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(planning_scene::PlanningSceneConstPtr env,
                                                                   const robot_state::JointModelGroup* joint_model_group,
                                                                   const sco::VarVector& vars)
  : CollisionEvaluator(env, joint_model_group), m_vars(vars)
{
}

void SingleTimestepCollisionEvaluator::CalcCollisions(const sco::DblVec& x, collision_detection::DistanceMap& distance_results)
{
  sco::DblVec dofvals = getDblVec(x, m_vars);

  auto diff = env->diff();
  const collision_detection::WorldPtr world_ptr = diff->getWorldNonConst();
  const robot_model::RobotModelConstPtr robot_model_ptr = diff->getRobotModel();
  moveit::core::RobotState robot_state(robot_model_ptr);
  robot_state.setVariablePositions(joint_model_group->getActiveJointModelNames(), dofvals);
  robot_state.update();

  auto dreq = collision_detection::DistanceRequest();
  auto dres = collision_detection::DistanceResult();
  dreq.group_name = joint_model_group->getName();
  dreq.enable_signed_distance = true;
  dreq.enable_nearest_points = true;
  dreq.acm = &diff->getAllowedCollisionMatrix();
  dreq.type = collision_detection::DistanceRequestType::SINGLE;
  collision_detection::CollisionEnvFCL cenv(robot_model_ptr, world_ptr);

  cenv.distanceRobot(dreq, dres, robot_state);
  distance_results.insert(dres.distances.begin(), dres.distances.end());
  cenv.distanceSelf(dreq, dres, robot_state);
  distance_results.insert(dres.distances.begin(), dres.distances.end());
}

void SingleTimestepCollisionEvaluator::CalcDists(const sco::DblVec& x, sco::DblVec& dists)
{
  collision_detection::DistanceMap distance_results;
  GetCollisionsCached(x, distance_results);
  CollisionsToDistances(distance_results, dists);
}

void SingleTimestepCollisionEvaluator::CalcDistExpressions(const sco::DblVec& x, sco::AffExprVector& exprs)
{
  collision_detection::DistanceMap distance_results;
  GetCollisionsCached(x, distance_results);
  sco::DblVec dofvals = sco::getDblVec(x, m_vars);
  CollisionsToDistanceExpressions(distance_results, env, joint_model_group, m_vars, dofvals, exprs, false);
}

//////////////////////////////////////////

CollisionCost::CollisionCost(planning_scene::PlanningSceneConstPtr env,
                             const robot_state::JointModelGroup* joint_model_group,
                             double dist_pen, double coeff, const sco::VarVector& vars)
  : Cost("collision"), m_calc(new SingleTimestepCollisionEvaluator(env, joint_model_group, vars)), m_dist_pen(dist_pen), m_coeff(coeff)
{
}

sco::ConvexObjectivePtr CollisionCost::convex(const sco::DblVec& x, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  sco::AffExprVector exprs;
  m_calc->CalcDistExpressions(x, exprs);
  for (std::size_t i=0; i < exprs.size(); ++i)
  {
    sco::AffExpr viol = sco::exprSub(sco::AffExpr(m_dist_pen), exprs[i]);
    out->addHinge(viol, m_coeff);
  }
  return out;
}

double CollisionCost::value(const sco::DblVec& x)
{
  sco::DblVec dists;
  m_calc->CalcDists(x, dists);
  double out = 0;
  for (std::size_t i=0; i < dists.size(); ++i) {
    out += sco::pospart(m_dist_pen - dists[i]) * m_coeff;
  }
  return out;
}

// ALMOST EXACTLY COPIED FROM CollisionCost

CollisionConstraint::CollisionConstraint(planning_scene::PlanningSceneConstPtr env,
                                         const robot_state::JointModelGroup* joint_model_group,
                                         double dist_pen, double coeff, const sco::VarVector& vars)
  : m_calc(new SingleTimestepCollisionEvaluator(env, joint_model_group, vars)), m_dist_pen(dist_pen), m_coeff(coeff)
{
  name_ = "collision";
}

sco::ConvexConstraintsPtr CollisionConstraint::convex(const sco::DblVec& x, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  sco::AffExprVector exprs;
  m_calc->CalcDistExpressions(x, exprs);
  for (std::size_t i = 0; i < exprs.size(); ++i)
  {
    sco::AffExpr viol = sco::exprSub(sco::AffExpr(m_dist_pen), exprs[i]);
    out->addIneqCnt(sco::exprMult(viol, m_coeff));
  }
  return out;
}

sco::DblVec CollisionConstraint::value(const sco::DblVec& x) {
  sco::DblVec dists;
  m_calc->CalcDists(x, dists);
  sco::DblVec out(dists.size());
  for (std::size_t i=0; i < dists.size(); ++i) {
    out[i] = sco::pospart(m_dist_pen - dists[i]) * m_coeff;
  }
  return out;
}

}  // namespace trajopt


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

// TestCorrelator.h
#include <gtest/gtest.h>

#include <vw/Image/UtilityViews.h>
#include <vw/Stereo/CorrelatorView.h>
#include <vw/Image/Transform.h>

#include <boost/random/linear_congruential.hpp>

using namespace vw;
using namespace vw::stereo;

class BasicCorrelationTest : public ::testing::Test {
protected:
  BasicCorrelationTest() {}

  virtual void SetUp() {
    boost::rand48 gen(10);
    image1 = 255*uniform_noise_view( gen, 50, 50 );
    image2 = transform(image1, TranslateTransform(3,3),
                       ZeroEdgeExtension(), NearestPixelInterpolation());
    mask.set_size(50,50);
    fill(mask,PixelMask<uint8>(255));
  }

  template <class ViewT, class MViewT, class PreProcT>
  CorrelatorView<typename ViewT::pixel_type,
                 typename MViewT::pixel_type,
                 PreProcT>
  correlate( ImageViewBase<ViewT> const& input1,
             ImageViewBase<ViewT> const& input2,
             ImageViewBase<MViewT> const& mask,
             PreProcT const& proc,
             stereo::CorrelatorType type = stereo::ABS_DIFF_CORRELATOR ) {
    typedef typename ViewT::pixel_type pixel_type;
    typedef typename MViewT::pixel_type mask_type;
    CorrelatorView<pixel_type,mask_type,PreProcT> corr( input1, input2, mask, mask, proc, false );
    corr.set_search_range( BBox2i(0,0,6,6) );
    corr.set_kernel_size(  Vector2i(7,7)   );
    corr.set_correlator_options( 1, type );
    return corr;
  }

  template <class ViewT>
  void check_error( ImageViewBase<ViewT> const& input,
                    float success = 0.9 ) {
    ViewT const& disparity_map = input.impl();
    int count_correct = 0;
    int count_valid = 0;
    for (int j = 0; j < disparity_map.rows(); ++j)
      for (int i = 0; i < disparity_map.cols(); ++i)
        if ( is_valid( disparity_map(i,j) ) ) {
          count_valid++;
          if ( disparity_map(i,j).child() == Vector2f(3,3) )
            count_correct++;
        }
    EXPECT_GT( float(count_correct)/float(count_valid), success );
  }

  ImageView<uint8> image1, image2;
  ImageView<PixelMask<uint8> > mask;
};

TEST_F( BasicCorrelationTest, NullPreprocess ) {
  typedef NullStereoPreprocessingFilter FilterT;

  ImageView<PixelMask<Vector2f> > disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::ABS_DIFF_CORRELATOR );
  check_error( disparity_map, 0.95 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::SQR_DIFF_CORRELATOR );
  check_error( disparity_map, 0.93 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::NORM_XCORR_CORRELATOR );
  check_error( disparity_map, 0.95 );
}

TEST_F( BasicCorrelationTest, SlogPreprocess ) {
  typedef SlogStereoPreprocessingFilter FilterT;

  ImageView<PixelMask<Vector2f> > disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::ABS_DIFF_CORRELATOR );
  check_error( disparity_map, 0.84 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::SQR_DIFF_CORRELATOR );
  check_error( disparity_map, 0.84 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::NORM_XCORR_CORRELATOR );
  check_error( disparity_map, 0.915 );
}

TEST_F( BasicCorrelationTest, LogPreprocess ) {
  typedef LogStereoPreprocessingFilter FilterT;

  ImageView<PixelMask<Vector2f> > disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::ABS_DIFF_CORRELATOR );
  check_error( disparity_map, 0.85 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::SQR_DIFF_CORRELATOR );
  check_error( disparity_map, 0.81 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::NORM_XCORR_CORRELATOR );
  check_error( disparity_map, 0.89 );
}

TEST_F( BasicCorrelationTest, BlurPreprocess ) {
  typedef BlurStereoPreprocessingFilter FilterT;

  ImageView<PixelMask<Vector2f> > disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::ABS_DIFF_CORRELATOR );
  check_error( disparity_map, 0.79 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::SQR_DIFF_CORRELATOR );
  check_error( disparity_map, 0.75 );

  disparity_map =
    correlate( image1, image2, mask, FilterT(),
               stereo::NORM_XCORR_CORRELATOR );
  check_error( disparity_map, 0.79 );
}


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

#include <boost/simd/operator/functions/plus.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( plus_, boost::simd::tag::qpx_
                                   , (A0)
                                   , ((simd_<double_<A0>, boost::simd::tag::qpx_>))
                                     ((simd_<double_<A0>, boost::simd::tag::qpx_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return vec_add(a0(), a1());
    }
  };
} } }

#endif
#endif


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_HYPERGEOMETRIC_1F0_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F0_HPP

#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost { namespace math { namespace detail {

template <class T, class Policy>
inline T hypergeometric_1F0_imp(const T& a, const T& z, const Policy& pol)
{
   static const char* function = "boost::math::hypergeometric_1F0<%1%,%1%>(%1%, %1%)";
   BOOST_MATH_STD_USING // pow

   if (z == 1)
      return policies::raise_pole_error<T>(
         function,
         "Evaluation of 1F0 with z = %1%.",
         z,
         pol);
   if (1 - z < 0)
   {
      if (floor(a) != a)
         return policies::raise_domain_error<T>(function,
            "Result is complex when a is non-integral and z > 1, but got z = %1%", z, pol);
   }
   // more naive and convergent method than series
   return pow(T(1 - z), T(-a));
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type hypergeometric_1F0(T1 a, T2 z, const Policy&)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::hypergeometric_1F0_imp<value_type>(
         static_cast<value_type>(a),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_1F0<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type hypergeometric_1F0(T1 a, T2 z)
{
   return hypergeometric_1F0(a, z, policies::policy<>());
}

  } } // namespace boost::math

#endif // BOOST_MATH_HYPERGEOMETRIC_1F0_HPP


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

// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2012 Bruno Lalande, Paris, France.
// Copyright (c) 2012 Mateusz Loskot, London, UK.

// 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_MULTI_ALGORITHMS_DISJOINT_HPP
#define BOOST_GEOMETRY_MULTI_ALGORITHMS_DISJOINT_HPP

#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/multi/algorithms/covered_by.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Point, typename MultiPolygon>
struct disjoint<Point, MultiPolygon, 2, point_tag, multi_polygon_tag, false>
    : detail::disjoint::reverse_covered_by<Point, MultiPolygon>
{};

} // namespace dispatch

#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_MULTI_ALGORITHMS_DISJOINT_HPP


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

    void writeAscii(FILE *fp) const {
        fprintf(fp, "%6d %2d %+e", this->id, this->istar, this->mass);         //  3
        fprintf(fp, " %+e %+e %+e", this->pos[0], this->pos[1], this->pos[2]); //  6
        fprintf(fp, " %+e %+e %+e", this->vel[0], this->vel[1], this->vel[2]); //  9
        fprintf(fp, " %+e %+e %+e", this->acc[0], this->acc[1], this->acc[2]); // 12
        fprintf(fp, " %+e %+e %+e", this->uene, this->alph, this->alphu);      // 15
        fprintf(fp, " %+e %+e %6d", this->dens, this->ksr,  this->np);         // 18
        fprintf(fp, " %+e %+e %+e", this->vsnd, this->pres, this->temp);       // 21
        fprintf(fp, " %+e %+e %+e", this->divv, this->rotv, this->bswt);       // 24
        fprintf(fp, " %+e %+e %+e", this->pot, this->abar, this->zbar);        // 27
        fprintf(fp, " %+e",         this->enuc);                               // 28
        fprintf(fp, " %+e %+e %+e", this->vsmx, this->udot, this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {        // 32 -- 44
            fprintf(fp, " %+.3e", this->cmps[k]);
        }
        if(RP::FlagDamping == 2) {
            PS::F64vec omg = RP::RotationalVelocity;
            PS::F64vec vec = omg ^ pos;
            fprintf(fp, " %+e", this->pot - 0.5 * (vec * vec));                // 45
        }
        fprintf(fp, " %+e", this->pot3);                                       // 45
        fprintf(fp, " %+e %+e %+e", this->tempmax[0], this->tempmax[1],
                this->tempmax[2]);                                             // 46 -- 48
        fprintf(fp, " %+e", this->entr);                                       // 49
        fprintf(fp, "\n");
    }

};

template <class Tsph>
void calcPositionAndVelocityOfDensityCenter(Tsph & sph,
                                            PS::F64vec & xcglb,
                                            PS::F64vec & vcglb) {
    PS::F64    dnloc = 0.;
    PS::F64vec xcloc = 0.;
    PS::F64vec vcloc = 0.;
    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        dnloc += sph[i].dens;
        xcloc += sph[i].dens * sph[i].pos;
        vcloc += sph[i].dens * sph[i].vel;
    }
    PS::F64 dnglb;
    dnglb = PS::Comm::getSum(dnloc);
    xcglb = PS::Comm::getSum(xcloc);
    vcglb = PS::Comm::getSum(vcloc);
    xcglb /= dnglb;
    vcglb /= dnglb;
}

template <class Tsph>
void findBoundParticle(char * hfile,
                       char * ofile,
                       PS::S64 itime,
                       PS::S64 searchmode,
                       PS::S64 printmode,
                       PS::S64 excludedID,
                       Tsph & sph) {
    PS::F64vec xc, vc;
    calcPositionAndVelocityOfDensityCenter(sph, xc, vc);

    NR::Nucleon mxloc;
    FILE * fp = NULL;
    if(printmode == 0) {
        fp = fopen(ofile, "w");
    }
    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        PS::F64vec dx = sph[i].pos - xc;
        PS::F64vec dv = sph[i].vel - vc;
        PS::F64    eb = 0.5 * (dv * dv) + sph[i].pot;
        eb = (searchmode == 0) ? eb : (- eb);
        //if(eb < 0.) {
        if(eb < 0. && !(sph[i].istar == excludedID)) {
            if(printmode == 0) {
                sph[i].writeAscii(fp);
            }
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                mxloc[k] += sph[i].mass * sph[i].cmps[k];
            }
        }
    }
    if(printmode == 0) {
        fclose(fp);
    }
    NR::Nucleon mxglb;
    for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
        mxglb[k]  = PS::Comm::getSum(mxloc[k]);
        mxglb[k] /= CodeUnit::SolarMass;
    }

    if(PS::Comm::getRank() == 0) {
        FILE * fp = fopen(hfile, "a");
        fprintf(fp, "%8d", itime);                                         // 1
        fprintf(fp, " %+e %+e %+e", xc[0], xc[1], xc[2]);                  // 4
        fprintf(fp, " %+e %+e %+e", vc[0], vc[1], vc[2]);                  // 7
        fprintf(fp, " %+.3e %+.3e %+.3e", mxglb[0], mxglb[1],  mxglb[2]);  // 10
        fprintf(fp, " %+.3e %+.3e %+.3e", mxglb[3], mxglb[4],  mxglb[5]);  // 13
        fprintf(fp, " %+.3e %+.3e %+.3e", mxglb[6], mxglb[7],  mxglb[8]);  // 16
        fprintf(fp, " %+.3e %+.3e %+.3e", mxglb[9], mxglb[10], mxglb[11]); // 19
        fprintf(fp, " %+.3e", mxglb[12]);                                  // 20
        fprintf(fp, "\n");
        fclose(fp);
    }

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::S64 ibgn, iend;
    PS::S64 searchmode;
    PS::S64 printmode;
    PS::S64 excludedID;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lld%lld", &ibgn, &iend);
    fscanf(fp, "%lld", &searchmode);
    fscanf(fp, "%lld", &printmode);
    fscanf(fp, "%lld", &excludedID);
    fclose(fp);

    char hfile[1024];
    sprintf(hfile, "%s/summary.log", odir);
    for(PS::S64 itime = ibgn; itime <= iend; itime++) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        //sprintf(ofile, "%s/sph_t%04d_p%06d_i%06d.dat.bound",
        sprintf(ofile, "%s/sph_t%04d_p%06d_i%06d.dat",
                odir, itime, PS::Comm::getNumberOfProc(), PS::Comm::getRank());
        findBoundParticle(hfile, ofile, itime, searchmode, printmode, excludedID, sph);

    }

    PS::Finalize();

    return 0;
}


#ifndef SAST_SAST_HPP
#define SAST_SAST_HPP

#include <stack>
#include <stdexcept>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/range/adaptor/sliced.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/iterator.hpp>
#include <boost/range/size.hpp>
#include <boost/range/sub_range.hpp>
#include "esa.hxx"

namespace sast {

template <class RandomAccessRange, class Index>
struct positional_finder;

template <class RandomAccessRange, class Index>
struct sast;

template <class RandomAccessRange, class Index>
positional_finder<RandomAccessRange, Index> make_positional_finder(const sast<RandomAccessRange, Index>&);

template <class RandomAccessRange, class Index>
struct sast {
    using range_type = RandomAccessRange;
    using char_type = typename boost::range_value<RandomAccessRange>::type;
    using index_type = Index;

    struct substr {
        using iterator       = typename boost::range_iterator<RandomAccessRange>::type;
        using const_iterator = typename boost::range_const_iterator<RandomAccessRange>::type;

        index_type pos()       const { return parent_->sa_[parent_->l_[i_]]; }
        boost::sub_range<const std::vector<index_type>> allpos() const { return boost::adaptors::slice(parent_->sa_, parent_->l_[i_], parent_->r_[i_]); }
        index_type length()    const { return parent_->d_[i_]; }
        index_type frequency() const { return parent_->r_[i_] - parent_->l_[i_]; }

        iterator begin() { return boost::begin(parent_->input_) + pos(); }
        iterator end()   { return boost::begin(parent_->input_) + pos() + length(); }
        const_iterator begin() const { return boost::const_begin(parent_->input_) + pos(); }
        const_iterator end()   const { return boost::const_begin(parent_->input_) + pos() + length(); }

        substr(const sast* parent, int i)
            : parent_(parent), i_(i)
        {}

    private:
        const sast* parent_;
        int i_;
    };

private:
    template <class> struct node_iterator;

public:
    using iterator       = node_iterator<substr>;
    using const_iterator = node_iterator<const substr>;

    sast(const RandomAccessRange& input, const size_t alphabet_size)
        : input_(input),
          sa_(boost::size(input)),
          l_(boost::size(input)),
          r_(boost::size(input)),
          d_(boost::size(input)),
          node_to_parent_node_()
    {
        // suffix array
        int err = esaxx(boost::const_begin(input_),
                        sa_.begin(),
                        l_.begin(), r_.begin(), d_.begin(),
                        static_cast<index_type>(boost::size(input_)),
                        static_cast<index_type>(alphabet_size),
                        num_nodes_);
        if (err) throw std::runtime_error("saisxx failed to construct a suffix array.");

        // Dummy node
        // These values are designed so that they can be used as well as those
        // of the other nodes.
        l_[num_nodes_] = 0;
        r_[num_nodes_] = 0;
        d_[num_nodes_] = 0;

        // node_to_parent_node[i]: ノードiの親ノードの番号（post-order）。
        // suffix_to_parent_node[k]: 接尾辞input[k..$]に対応する葉ノードの、親ノードのpost-order順の番号。
        node_to_parent_node_.resize(num_nodes_);
        std::vector<index_type> suffix_to_parent_node(boost::size(input) + 1);
        suffix_to_parent_node[boost::size(input)] = num_nodes_ - 1;  // 接尾辞input[$..$]
        {
            std::stack<index_type> stk;  // the top of the stack is a current parent node
            stk.push(num_nodes_);  // put the dummy node, which will be the parent of the root node
            index_type next_node = num_nodes_ - 1;  // a node to consider next
            index_type i = boost::size(input) - 1;  // a current suffix, the i-th suffix in the suffix array
            // narrow the range [l, r) to find the immediate parent of the i-th node
            while (next_node >= 0 && l_[next_node] <= i && i < r_[next_node]) {
                node_to_parent_node_[next_node] = stk.top();
                stk.push(next_node);
                --next_node;
            }
            while (i >= 0) {
                // widen the range [l, r) to find the lowest ancestor of the i-th node
                while (!(l_[stk.top()] <= i && i < r_[stk.top()])) {
                    stk.pop();
                }
                // narrow the range [l, r) to find the immediate parent of the i-th node
                while (next_node >= 0 && l_[next_node] <= i && i < r_[next_node]) {
                    node_to_parent_node_[next_node] = stk.top();
                    stk.push(next_node);
                    --next_node;
                }
                suffix_to_parent_node[sa_[i]] = stk.top();
                --i;
            }
        }

        // suffix_link_
        suffix_link_.resize(num_nodes_);
        suffix_link_[num_nodes_ - 1] = num_nodes_;  // transfers to the dummy node.
        for (int i = 0; i < num_nodes_ - 1; ++i) {
            // ここではノードi（iはpost-orderでの番号）に対応する部分文字列substrを扱う。
            const auto len_substr  = d_[i];
            const auto pos_substr  = sa_[l_[i]];
            // substrの先頭を1文字削ったsub-substrを考える。
            const auto len_subsubstr = len_substr - 1;
            // sub-substrに対応するノードを見つける。
            // {内部,葉}ノードに対応する部分文字列から先頭の1文字を削って得られ
            // る文字列には、必ず対応する{内部,葉}ノードが存在する。
            auto j = suffix_to_parent_node[pos_substr + 1];  // 接尾辞input[(pos_substr + j)..$]に対応する葉ノードの親ノード
            while (d_[j] > len_subsubstr) j = node_to_parent_node_[j];  // d[j] == len_subsubstr ならば、ノードjはsub-substrに対応するノード。
            suffix_link_[i] = j;
        }
    }

    iterator begin() { return iterator(this, 0); }
    iterator end()   { return iterator(this, num_nodes_); }
    const_iterator begin() const { return const_iterator(this, 0); }
    const_iterator end()   const { return const_iterator(this, num_nodes_); }

    index_type size() const { return num_nodes_; }

    const RandomAccessRange& input() const {
        return input_;
    }

    friend positional_finder<RandomAccessRange, index_type> make_positional_finder<>(const sast&);

private:
    template <class Value>
    struct node_iterator
        : public boost::iterator_facade<
            node_iterator<Value>,
            Value,
            boost::random_access_traversal_tag,
            Value,
            int>
    {
        node_iterator()
            : parent_(0), i_(-1)
        {}

        node_iterator(const sast* parent, int i)
            : parent_(parent), i_(i)
        {}

        template <class OtherValue>
        node_iterator(node_iterator<OtherValue> const& other)
            : parent_(other.parent_), i_(other.i_)
        {}

        node_iterator<Value> parent() {
            return node_iterator(parent_, parent_->node_to_parent_node_[i_]);
        }

        node_iterator<Value> suffix() {
            return node_iterator(parent_, parent_->suffix_link_[i_]);
        }

    private:
        friend class boost::iterator_core_access;
        template <class> friend struct node_iterator;

        void increment() { ++i_; }
        void decrement() { --i_; }
        void advance(int n) { i_ += n; }
        int distance_to(const node_iterator<Value>& other) const { return other.i_ - this->i_; }

        template <class OtherValue>
        bool equal(const node_iterator<OtherValue>& other) const {
            return this->parent_ == other.parent_ && this->i_ == other.i_;
        }

        Value dereference() const {
            return substr(parent_, i_);
        }

        const sast* parent_;
        int i_;
    };

    const RandomAccessRange& input_;
    std::vector<index_type> sa_;
    std::vector<index_type>  l_;
    std::vector<index_type>  r_;
    std::vector<index_type>  d_;
    index_type  num_nodes_;
    std::vector<index_type> node_to_parent_node_;
    std::vector<index_type> suffix_link_;
};

template <class RandomAccessRange, class Index>
struct positional_finder {
    using range_type = RandomAccessRange;
    using char_type = typename boost::range_value<RandomAccessRange>::type;
    using index_type = Index;

private:
    using sast_type = sast<RandomAccessRange, Index>;

public:
    template <class Vector>
    positional_finder(const sast_type& sast, Vector&& suffix_to_parent_node)
        : sast_(sast), suffix_to_parent_node_(std::forward<Vector>(suffix_to_parent_node))
    {}

    typename sast_type::iterator find(const int i, const int j) const {
        const auto len_substr = j - i;
        const auto pos_substr = i;
        // substrに対応する内部ノードを見つける。
        auto n = sast_.begin() + suffix_to_parent_node_[pos_substr];  // 接尾辞input[pos_substr..$]に対応する葉ノードの親ノード
        if (n->length() >= len_substr) {
            // substrは2回以上出現しており、対応する内部ノードが存在する。
            // d[node_to_parent_node[k]] < len_substr <= d[k] を満たす k を
            // 見つける。
            while (n.parent()->length() >= len_substr) {
                n = n.parent();
            }
            // const auto kk = n->length() - len_substr;  // ノードkではkk文字削ったことに相当する。

            return n;
        }
        else {
            // substrは1回しか出現しておらず、対応する内部ノードが存在しない。
            return n;
        }
    }

private:
    const sast_type& sast_;
    const std::vector<Index> suffix_to_parent_node_;
};

template <class RandomAccessRange, class Index>
positional_finder<RandomAccessRange, Index> make_positional_finder(const sast<RandomAccessRange, Index>& sast) {
    const auto& input = sast.input_;
    const auto& sa_ = sast.sa_;
    const auto& l_ = sast.l_;
    const auto& r_ = sast.r_;
    const auto& num_nodes_ = sast.num_nodes_;

    // suffix_to_parent_node[k]: 接尾辞input[k..$]に対応する葉ノードの、親ノードのpost-order順の番号。
    std::vector<Index> suffix_to_parent_node(boost::size(input) + 1);
    suffix_to_parent_node[boost::size(input)] = num_nodes_ - 1;  // 接尾辞input[$..$]
    {
        std::stack<Index> stk;  // the top of the stack is a current parent node
        stk.push(num_nodes_);  // put the dummy node, which will be the parent of the root node
        Index next_node = num_nodes_ - 1;  // a node to consider next
        Index i = boost::size(input) - 1;  // a current suffix, the i-th suffix in the suffix array
        // narrow the range [l, r) to find the immediate parent of the i-th node
        while (next_node >= 0 && l_[next_node] <= i && i < r_[next_node]) {
            stk.push(next_node);
            --next_node;
        }
        while (i >= 0) {
            // widen the range [l, r) to find the lowest ancestor of the i-th node
            while (!(l_[stk.top()] <= i && i < r_[stk.top()])) {
                stk.pop();
            }
            // narrow the range [l, r) to find the immediate parent of the i-th node
            while (next_node >= 0 && l_[next_node] <= i && i < r_[next_node]) {
                stk.push(next_node);
                --next_node;
            }
            suffix_to_parent_node[sa_[i]] = stk.top();
            --i;
        }
    }

    return positional_finder<RandomAccessRange, Index>(sast, std::move(suffix_to_parent_node));
}

}  // namespace sast

#endif  /* SAST_SAST_HPP */