/******************************************************************************
 * Copyright (c) 2011
 * Alexey Zakharov, Yury Brodskiy
 *
 *
 * This software is published under a dual-license: GNU Lesser General Public
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 * * Neither the name of company nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ******************************************************************************/

#include <boost/units/systems/si.hpp>
#include <boost/units/physical_dimensions.hpp>
#include <boost/units/io.hpp>
#include <geometry_msgs/Quaternion.h>

#include "cartesian_compliance_control.h"
#include "20_sim_cartesian_compliance_control/common/xxmatrix.h"
#include "pluginlib/class_list_macros.h"
#include <tf/transform_datatypes.h>

PLUGINLIB_DECLARE_CLASS(youbot_description, CartesianComplianceController, controller::CartesianComplianceController, pr2_controller_interface::Controller)

using namespace std;

namespace controller {

CartesianComplianceController* CartesianComplianceControllerDebug::getControllerPtr() {
	return dynamic_cast<CartesianComplianceController*> (this->controllerPtr);
}

void CartesianComplianceControllerDebug::toTransformMatrix(double* tf, tf::Transform& trans) {
	trans.setOrigin(tf::Vector3(tf[3], tf[7], tf[11]));
	tf::Matrix3x3 rotMatrix(tf[0], tf[1], tf[2],
			tf[4], tf[5], tf[6],
			tf[8], tf[9], tf[10]);
	tf::Quaternion quat;
	rotMatrix.getRotation(quat);
	trans.setRotation(quat);
}

void CartesianComplianceControllerDebug::publishTf(double* tf, string parent, string child) {
	/*tf::Transform trans;
	trans.setOrigin(tf::Vector3(tf[3], tf[7], tf[11]));
	tf::Matrix3x3 rotMatrix(tf[0], tf[1], tf[2],
			tf[4], tf[5], tf[6],
			tf[8], tf[9], tf[10]);
	tf::Quaternion quat;
	rotMatrix.getRotation(quat);
	trans.setRotation(quat);*/
	tf::Transform trans;
	toTransformMatrix(tf, trans);
	br.sendTransform(tf::StampedTransform(trans, ros::Time::now(), parent, child));
}

void CartesianComplianceControllerDebug::init() {

	ros::NodeHandle &nodeHandle = getControllerPtr()->nodeHandle;
/*	gazeboJointPositions.reset(new realtime_tools::RealtimePublisher<brics_actuator::JointPositions > (nodeHandle, "gazebo_joint_positions", 1));
	gazeboJointPositions->lock();
	controllerJointPositions.reset(new realtime_tools::RealtimePublisher<brics_actuator::JointPositions > (nodeHandle, "controller_joint_positions", 1));
	controllerJointPositions->lock();
	gazeboJointTorques.reset(new realtime_tools::RealtimePublisher<brics_actuator::JointTorques > (nodeHandle, "gazebo_joint_torques", 1));
	gazeboJointTorques->lock();
	controllerJointTorques.reset(new realtime_tools::RealtimePublisher<brics_actuator::JointTorques > (nodeHandle, "controller_joint_torques", 1));
	controllerJointTorques->lock();
	gazeboJointPose.reset(new realtime_tools::RealtimePublisher<brics_actuator::CartesianPose > (nodeHandle, "gazebo_joint_pose", 1));
	gazeboJointPose->lock();
	controllerJointPose.reset(new realtime_tools::RealtimePublisher<brics_actuator::CartesianPose > (nodeHandle, "controller_joint_pose", 1));
	controllerJointPose->lock();
*/
    currentTipPose.reset(new realtime_tools::RealtimePublisher<brics_actuator::CartesianPose> (nodeHandle, "current_tip_pose", 1));
    targetTipPose.reset(new realtime_tools::RealtimePublisher<brics_actuator::CartesianPose> (nodeHandle, "target_tip_pose", 1));
}

void CartesianComplianceControllerDebug::publish() {

	/*vector <brics_actuator::JointValue> positionsGazebo;
	vector <brics_actuator::JointValue> positionsController;
	vector <brics_actuator::JointValue> torquesGazebo;*/

	CartesianComplianceController* ctrlPtr = getControllerPtr();

	/*unsigned int size = ctrlPtr->joints.size();
	positionsGazebo.resize(size);
	positionsController.resize(size);
	torquesGazebo.resize(size);*/

	XXMatrix* matrix = ctrlPtr->autoGenerated20simController.m_M;
	double* joint1 = matrix[42].mat;
	publishTf(joint1, "/odom", "/joint1");
	double* joint2 = matrix[43].mat;
	publishTf(joint2, "/odom", "/joint2");
	double* joint3 = matrix[44].mat;
	publishTf(joint3, "/odom", "/joint3");
	double* joint4 = matrix[45].mat;
	publishTf(joint4, "/odom", "/joint4");
	double* joint5 = matrix[46].mat;
	publishTf(joint5, "/odom", "/joint5");
	double* tip = matrix[2].mat;
	publishTf(tip, "/odom", "/tip");

//	std::vector<pr2_mechanism_model::JointState*>::iterator joinsIterator;

	using namespace boost::units;

//	unsigned int j = 0;

/*	for (joinsIterator = ctrlPtr->joints.begin(); joinsIterator != ctrlPtr->joints.end(); ++joinsIterator) {
		positionsGazebo[j].joint_uri = (*joinsIterator)->joint_->name;
		positionsGazebo[j].value = (*joinsIterator)->position_;
		positionsGazebo[j].unit = to_string(si::radians);
		positionsGazebo[j].timeStamp = ctrlPtr->currentTime;

		positionsController[j].joint_uri = (*joinsIterator)->joint_->name;
		//positionsController[j].value = u[28+j]; //CHANGE THAT!
		positionsController[j].unit = to_string(si::radians);
		positionsController[j].timeStamp = ctrlPtr->currentTime;

		torquesGazebo[j].joint_uri = (*joinsIterator)->joint_->name;
		torquesGazebo[j].value = (*joinsIterator)->commanded_effort_;
		torquesGazebo[j].unit = to_string(si::newton_meter);
		torquesGazebo[j].timeStamp = ctrlPtr->currentTime;
		++j;
	}

	gazeboJointPositions->msg_.positions = positionsGazebo;
	controllerJointPositions->msg_.positions = positionsController;
	gazeboJointTorques->msg_.torques = torquesGazebo;

	gazeboJointPositions->unlockAndPublish();
	controllerJointPositions->unlockAndPublish();
	gazeboJointTorques->unlockAndPublish();
*/
//    tf::Transform trans;
//	toTransformMatrix(tf, trans);

    currentTipPose->msg_ = ctrlPtr->currentTipPose;
    targetTipPose->msg_ = ctrlPtr->targetTipPose;
    currentTipPose->unlockAndPublish();
    targetTipPose->unlockAndPublish();
}

CartesianComplianceController::CartesianComplianceController()
: robotPtr(NULL) {
    locked = true;
	loopCount = 0;
	this->position[0] = 0;
	this->position[1] = 0;
	this->position[2] = 0;

	this->orientationYPR[0] = 0;
	this->orientationYPR[1] = 0;
	this->orientationYPR[2] = 0;
#ifdef DEBUG_INFO
	debugInfo = new CartesianComplianceControllerDebug(this);

#else
	debugInfo = new Debug(this);
#endif

}

CartesianComplianceController::~CartesianComplianceController() {
	subscriber.shutdown();
	autoGenerated20simController.Terminate(u, y);
	delete debugInfo;
}

/*  auto-generated by 20sim;
	initialize the inputs and outputs with correct initial values */
void CartesianComplianceController::init20SimController() {

	/* initialize the inputs and outputs with correct initial values */
	//angles of the joionts
	u[0] = 0.0; //is not active
	u[1] = 0.0; //is not active
	u[2] = 0.0; //is not active
	u[3] = -170 * M_PI / 180; //J1
	u[4] = -65 * M_PI / 180; //J2
	u[5] = -146 * M_PI / 180; //J3
	u[6] = -102.5 * M_PI / 180; //J4
	u[7] = -167.5 * M_PI / 180; //J5
	/* velocities of the joints used for active dumping*/
	u[8] = 0.0; //is not active
	u[9] = 0.0; //is not active
	u[10] = 0.0; //is not active
	u[11] = 0.0; //J1
	u[12] = 0.0; //J2
	u[13] = 0.0; //J3
	u[14] = 0.0; //J4
	u[15] = 0.0; //J5
	u[16] = 0.0; /* xyzrpy *///not tested
	u[17] = 0.0;
	u[18] = 0.53;
	u[19] = 0.0;
	u[20] = 0.0;
	u[21] = 0.0;

	y[0] = 0.0; /* output */
	y[1] = 0.0;
	y[2] = 0.0;
	y[3] = 0.0;
	y[4] = 0.0;
	y[5] = 0.0;
	y[6] = 0.0;
	y[7] = 0.0;
	y[8] = 0.0;
	y[9] = 0.0;
	y[10] = 0.0;
	y[11] = 0.0;
	y[12] = 0.0;
	y[13] = 0.0;
	y[14] = 0.0;
	y[15] = 0.0;
	y[16] = 0.0; /* p.e */
	y[17] = 0.0;
	y[18] = 0.0;
	y[19] = 0.0;
	y[20] = 0.0;
	y[21] = 0.0;
	y[22] = 0.0;
	y[23] = 0.0;

	ROS_INFO("20sim init.\n");
	autoGenerated20simController.Initialize(u, y, 0.0);

}

bool CartesianComplianceController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle) {
	using namespace XmlRpc;
	this->nodeHandle = nodeHandle;
	this->robotPtr = robotPtr;

	ROS_INFO("Initializing interaction control for the youbot arm...\n");

	// Gets all of the joint pointers from the RobotState to a joints vector
	XmlRpc::XmlRpcValue jointNames;
	if (!nodeHandle.getParam("joints", jointNames)) {
		ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
		return false;
	}

	if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray) {
		ROS_ERROR("Malformed joint specification.  (namespace: %s)", nodeHandle.getNamespace().c_str());
		return false;
	}

	for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i) {
		XmlRpcValue &name = jointNames[i];
		if (name.getType() != XmlRpcValue::TypeString) {
			ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", nodeHandle.getNamespace().c_str());
			return false;
		}

		pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
		if (jointStatePtr == NULL) {
			ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
			return false;
		}

		joints.push_back(jointStatePtr);
	}

	// Ensures that all the joints are calibrated.
	for (unsigned int i = 0; i < joints.size(); ++i) {
		if (!joints[i]->calibrated_) {
			ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
			return false;
		}
	}

	// Initializing target efforts vector
	targetEfforts.resize(joints.size());

	// Subscribing for an input pose command
	subscriber = nodeHandle.subscribe("command", 1, &CartesianComplianceController::positionCommand, this);

	// Initializing 20Sim controller
	init20SimController();

	// Initializing twist publisher for the base
	ROS_INFO("base_confghfhgtroller/command\n");
	baseTwist.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist > (nodeHandle, "base_controller/command", 1));
	baseTwist->lock();

	subscriberOdometry = nodeHandle.subscribe("base_odometry/odometry", 1, &CartesianComplianceController::odometryCommand, this);

	// Initializing debug output
	debugInfo->init();

	return true;
}

void CartesianComplianceController::starting() {
	// Initializing timer
	currentTime = robotPtr->getTime();
	lastTime = currentTime;
}

void CartesianComplianceController::update20SimControl() {

    if (!locked) {
        tf::Quaternion quat;
        tf::quaternionMsgToTF(currentBasePose.orientation, quat);

        double ypr[3];
        tf::Matrix3x3(quat).getEulerYPR(ypr[0], ypr[1], ypr[2]);

        u[0] = ypr[0]; //is not active
     //  ROS_INFO("X: %f\n", u[0]);
    }

	u[1] = currentBasePose.position.x; //is not active
	u[2] = currentBasePose.position.y; //is not active
	u[3] = joints[0]->position_ - 170 * M_PI / 180; // -170 +170, clockwise
	u[4] = joints[1]->position_ - 65 * M_PI / 180; // -65  +90,  counterclockwis
	u[5] = -joints[2]->position_ - 146 * M_PI / 180; // +146 -151, clockwise
	//ROS_INFO("joint 3: %f", u[5]);
	u[6] = joints[3]->position_ - 102.5 * M_PI / 180; // -102 +102, counterclockwise
	u[7] = joints[4]->position_ - 167.5 * M_PI / 180; // -167 +167, clockwise
	/* velocities of the joints used for active dumping*/

	u[8] = 0.0;//currentBaseTwist.angular.z; //is not active
	u[9] = 0.0;//currentBaseTwist.linear.y;//0.0; //is not active
	u[10] = 0.0;//currentBaseTwist.linear.x;//0.0; //is not active
	//ROS_INFO("currentBaseTwist: x=%f, y=%f, z=%f\n", u[10], u[9], u[8]);

	u[11] = 0;//joints[0]->velocity_; //J1
	u[12] = 0;//joints[1]->velocity_; //J2
	u[13] = 0;//joints[2]->velocity_; //J3
	u[14] = 0;//joints[3]->velocity_; //J4
	u[15] = 0;//joints[4]->velocity_; //J5

	u[16] = this->position[0]; //0;0.0;            /* xyzYawPitchRoll *///not tested
	u[17] = this->position[1]; //0;0.2;
	u[18] = this->position[2]; //0,53;0.4;
	u[19] = this->orientationYPR[0];
	u[20] = this->orientationYPR[1]; //1.57;1.57;
	u[21] = this->orientationYPR[2]; //0.7;-0.7;-0.7

	autoGenerated20simController.Calculate(u, y);

	targetSpeed[0] = y[16]; // rot
	targetSpeed[1] = y[17]; // y
	targetSpeed[2] = y[18]; // x
    ROS_INFO("joint 3: %f", y[21]);
	targetEfforts[0] = y[19]; // -170 +170, clockwise
	targetEfforts[1] = y[20]; // -65  +90,  counterclockwise
	targetEfforts[2] = y[21]; // +146 -151, clockwise
	targetEfforts[3] = y[22]; // -102 +102, counterclockwise
	targetEfforts[4] = y[23]; // -167 +167, clockwise

}

void CartesianComplianceController::update() {

	currentTime = robotPtr->getTime();
	ros::Duration dt = currentTime - lastTime;
	lastTime = currentTime;

	// Initializing error vector
	if (!locked) //{
		update20SimControl();
	//ROS_INFO ("Current speed is: %f, %f\n", currentBaseTwist.angular.z, currentBaseTwist.linear.y);

	// } else
	//   ROS_INFO ("locked\n");

  //  ROS_INFO("vel: %f, %f, %f, %f, %f\n",joints[0]->velocity_,joints[1]->velocity_,joints[2]->velocity_,joints[3]->velocity_,joints[4]->velocity_);
	// Doing control here, calculating and applying the efforts
	for (unsigned int i = 0; i < joints.size(); ++i) {
		joints[i]->commanded_effort_ += targetEfforts[i] * 10.0;
	}

	// Sending control comands for the base

	//gazeboJointPositions->msg_.positions = positionsGazebo;
	//baseTwist->msg_ = ;
	geometry_msgs::Twist twist;
	baseTwist->trylock();

    twist.linear.x = targetSpeed[2] / 15.0;
    twist.linear.y = targetSpeed[1] / 15.0;
	//twist.angular.x = 0.0;//targetSpeed[2];
    //twist.angular.y = 0.0;//targetSpeed[1];
    twist.angular.z = targetSpeed[0] / 15.0 ;

	//ROS_INFO("Speed: %f, %f, %f\n", twist.angular.x, twist.angular.y, twist.angular.z);

	baseTwist->msg_ = twist;
	baseTwist->unlockAndPublish();

	// if debug mode is set, publishing debug TFs and controller information
	if (loopCount % 10 == 0) {
		debugInfo->publish();
	}
	++loopCount;
}

void CartesianComplianceController::positionCommand(const brics_actuator::CartesianPose &pose) {

	using namespace boost::units;

    targetTipPose = pose;

	brics_actuator::CartesianVector tipPosition;
	geometry_msgs::Quaternion tipOrientation;
	tipPosition = pose.position;
	tipOrientation = pose.orientation;

	if (tipPosition.unit != to_string(si::meter))
		ROS_ERROR("Position value is set in the inpcompatible units %s, expecting meters", tipPosition.unit.c_str());

	this->position[0] = tipPosition.x;
	this->position[1] = tipPosition.y;
	this->position[2] = tipPosition.z;

	tf::Quaternion quaternion;
	tf::quaternionMsgToTF(tipOrientation, quaternion);
	tf::Matrix3x3(quaternion).getEulerYPR(orientationYPR[0], orientationYPR[1], orientationYPR[2]);

}

void CartesianComplianceController::odometryCommand(const nav_msgs::Odometry &odometry) {
  //  ROS_INFO ("Odom: %f, %f, %f\n", currentBasePose.position.x, currentBasePose.position.y, currentBasePose.position.z);
  //  ROS_INFO ("Odom: %f, %f, %f, %f\n", currentBasePose.orientation.x, currentBasePose.orientation.y, currentBasePose.orientation.z,  currentBasePose.orientation.w);
	locked = true;
	currentBaseTwist = odometry.twist.twist;
	currentBasePose = odometry.pose.pose;
	locked = false;

}

}


#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <boost/bind.hpp>
#include <boost/asio.hpp>
#include <thread>
#include <string>
#include <vector>
#include <functional>
#include <mutex>
#include <realtime_tools/realtime_publisher.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Float32.h>
#include <eigen3/Eigen/Dense>

using namespace std;
using namespace boost;

using boost::asio::ip::udp;

const string JOINT_NAME[40] = {"WaistPitch","WaistYaw",
                               "R_ShoulderPitch","R_ShoulderRoll","R_ShoulderYaw","R_ElbowRoll","R_WristYaw","R_WristRoll","R_HandYaw",
                               "L_ShoulderPitch","L_ShoulderRoll","L_ShoulderYaw","L_ElbowRoll","L_WristYaw","L_WristRoll","L_HandYaw",
                               "R_HipYaw","R_HipRoll","R_HipPitch","R_KneePitch","R_AnklePitch","R_AnkleRoll",
                               "L_HipYaw","L_HipRoll","L_HipPitch","L_KneePitch","L_AnklePitch","L_AnkleRoll",
                               "HeadYaw", "HeadPitch", "R_Gripper", "L_Gripper"};

const int LEFT_LEG_START_NUM = 16;
const int RIGHT_LEG_START_NUM = 22;
const int MAX_JOINT = 32;

const double CNT_PER_REV = 983040.0;
const double CNT_PER_DEG = CNT_PER_REV / 360.0;
const double CNT_PER_RAD = CNT_PER_REV / ( M_PI * 2 );
const double RAD_PER_CNT = ( M_PI * 2 ) / CNT_PER_REV;

const double DSP_HZ = 1000.;

ros::Publisher joint_pub;

enum { max_length = 1024 };

uint8_t data[max_length];
uint8_t data2[max_length];
boost::asio::io_service io_service;
udp::socket udp_socket(io_service, udp::endpoint(udp::v4(), 1107));
udp::socket udp_socket2(io_service, udp::endpoint(udp::v4(), 1108));
udp::endpoint sender_endpoint;
udp::endpoint sender_endpoint2;

std::vector<double> q(12);
std::vector<double> qdot(12);

std::mutex data_mutex;

int32_t make_32s(uint8_t *data)
{
  return (data[0] << 8*3) + (data[1] << 8*2) + (data[2] << 8*1) + (data[3] << 8*0);
}
uint32_t make_32u(uint8_t *data)
{
  return (data[0] << 8*3) + (data[1] << 8*2) + (data[2] << 8*1) + (data[3] << 8*0);
}
int16_t make_16s(uint8_t *data)
{
  return (data[0] << 8*1) + (data[1] << 8*0);
}

void handle_receive_from(const boost::system::error_code& error,
    size_t bytes_recvd)
{
  if (!error && bytes_recvd > 50)
  {
    // SEND
    int startIndex = -1;
    if(data[1] == 'L')
    {
      // Leg
      if(data[0] == 'L')
      {
        // Left
        startIndex = 6;
      }
      else if(data[0] == 'R')
      {
        // Right
        startIndex = 0;
      }
    }

    if(startIndex == -1)
    {
      udp_socket.async_receive_from(
          boost::asio::buffer(data, max_length),
            sender_endpoint,handle_receive_from);
      return;
    }

    unsigned int timeStamp = make_32u(&data[2]);
    unsigned int seq = make_32u(&data[6]);
    int length = data[10];

    if(length != 6)
    {
      ROS_WARN("The length of the encoder values is not 6");
    }

    // CHECKSUM!!!!!!!!
    uint32_t checkSum = 0;
    uint32_t checkSumCalc = 0;
    for(int i=0; i<16+(length-1)*6; i++)
    {
        checkSum += data[i];
    }
    checkSumCalc = make_32u(&data[17+(length-1)*6]);
    if(checkSum != checkSumCalc)
    {
        ROS_WARN("Checksum error calc = %x, recv = %x", checkSum, checkSumCalc);
    }
    else
    {
        data_mutex.lock();
        for(int i=0; i<length; i++)
        {
          int q_cnt = make_32s(&data[11 + i * 6]);
          int qdot_cnt = make_16s(&data[15 + i * 6]);
          if (abs(q_cnt*RAD_PER_CNT) > 2.0)
          {
              ROS_WARN("Too much value idx: %d q: %lf", i, q_cnt*RAD_PER_CNT);
              for(int k=0; k<bytes_recvd; k++)
              {
                  printf("%d ", data[k]);
              }
              printf("\n");
          }
          else
          {
              q[startIndex + 5 - i] = q_cnt * RAD_PER_CNT;
              qdot[startIndex + 5 - i] = qdot_cnt * RAD_PER_CNT * DSP_HZ;
          }
        }
        data_mutex.unlock();
    }

  }
  udp_socket.async_receive_from(
      boost::asio::buffer(data, max_length),
        sender_endpoint,handle_receive_from);
}

void handle_receive_from2(const boost::system::error_code& error,
    size_t bytes_recvd)
{
  if (!error && bytes_recvd > 40)
  {
      // Protocol Left/Right Leg/Arm
    // SEND
    int startIndex = -1;
    if(data2[1] == 'L')
    {
      // Leg
      if(data2[0] == 'L')
      {
        // Left
        startIndex = 6;
      }
      else if(data2[0] == 'R')
      {
        // Right
        startIndex = 0;
      }
    }

    if(startIndex == -1)
    {
      udp_socket2.async_receive_from(
          boost::asio::buffer(data2, max_length),
            sender_endpoint2,handle_receive_from2);
      return;
    }

    unsigned int timeStamp = make_32u(&data2[2]);
    unsigned int seq = make_32u(&data2[6]);
    int length = data2[10];

    if(length != 6)
    {
      ROS_WARN("The length of the encoder values is not 6");
    }

    // CHECKSUM!!!!!!!!
    uint32_t checkSum = 0;
    uint32_t checkSumCalc = 0;
    for(int i=0; i<16+(length-1)*6; i++)
    {
        checkSum += data2[i];
    }
    checkSumCalc = make_32u(&data2[17+(length-1)*6]);
    if(checkSum != checkSumCalc)
    {
        ROS_WARN("Checksum error calc = %x, recv = %x", checkSum, checkSumCalc);
    }
    else
    {
        data_mutex.lock();
        for(int i=0; i<length; i++)
        {
          int q_cnt = make_32s(&data2[11 + i * 6]);
          int qdot_cnt = make_16s(&data2[15 + i * 6]);
          if (abs(q_cnt*RAD_PER_CNT) > 2.0)
          {
              ROS_WARN("Too much value idx: %d q: %lf", i, q_cnt*RAD_PER_CNT);
              for(int k=0; k<bytes_recvd; k++)
              {
                  printf("%d ", data2[k]);
              }
              printf("\n");
          }
          else
          {
              q[startIndex + 5 - i] = q_cnt * RAD_PER_CNT;
              qdot[startIndex + 5 - i] = qdot_cnt * RAD_PER_CNT * DSP_HZ;
          }
        }
        data_mutex.unlock();
    }

  }
  udp_socket2.async_receive_from(
      boost::asio::buffer(data2, max_length),
        sender_endpoint2,handle_receive_from2);
}

sensor_msgs::JointState joint_msg;

void joint_publish()
{
  data_mutex.lock();
  joint_msg.position = q;
  joint_msg.velocity = qdot;
  data_mutex.unlock();

  joint_pub.publish(joint_msg);

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "dyros_jet_ext_encoder_udp");
    ros::NodeHandle nh("~");

    joint_pub = nh.advertise<sensor_msgs::JointState>("/dyros_jet/ext_encoder", 5);
    udp_socket.async_receive_from(boost::asio::buffer(data, max_length), sender_endpoint,handle_receive_from);
    udp_socket2.async_receive_from(boost::asio::buffer(data2, max_length), sender_endpoint2,handle_receive_from2);
    boost::asio::io_service::work work(io_service);
    boost::thread thread(boost::bind(&boost::asio::io_service::run, &io_service));

    joint_msg.name.resize(12);
    joint_msg.position.resize(12);
    joint_msg.velocity.resize(12);

    for(int i=0; i<12; i++)
    {
      joint_msg.name[i] = JOINT_NAME[LEFT_LEG_START_NUM + i];
    }

    ros::Rate r(200);
    while(ros::ok())
    {
      joint_publish();
      ros::spinOnce();
      r.sleep();
    }

    return 0;
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "read_assigner.hpp"

#include <utility>
#include <iterator>
#include <algorithm>
#include <limits>
#include <stdexcept>
#include <cassert>

#include <boost/optional.hpp>

#include "utils/maths.hpp"
#include "utils/kmer_mapper.hpp"
#include "utils/random_select.hpp"
#include "core/models/haplotype_likelihood_model.hpp"
#include "core/models/error/error_model_factory.hpp"

namespace octopus {

namespace {

using HaplotypeLikelihoods = std::vector<std::vector<double>>;

auto vectorise(const Genotype<Haplotype>& genotype, const HaplotypeProbabilityMap& priors)
{
    std::vector<double> result(genotype.ploidy());
    std::transform(std::cbegin(genotype), std::cend(genotype), std::begin(result),
                   [&] (const auto& haplotype) { return priors.at(haplotype); });
    return result;
}

auto get_priors(const Genotype<Haplotype>& genotype, const HaplotypeProbabilityMap& log_priors)
{
    if (log_priors.empty()) {
        return std::vector<double>(genotype.ploidy());
    } else {
        return vectorise(genotype, log_priors);
    }
}

void find_map_haplotypes(const Genotype<Haplotype>& genotype, const unsigned read,
                         const HaplotypeLikelihoods& likelihoods, const std::vector<double>& log_priors,
                         std::vector<unsigned>& result)
{
    assert(result.empty());
    auto max_likelihood = std::numeric_limits<double>::lowest();
    for (unsigned k {0}; k < genotype.ploidy(); ++k) {
        const auto curr = likelihoods[k][read] + log_priors[k];
        if (maths::almost_equal(curr, max_likelihood)) {
            result.push_back(k);
        } else if (curr > max_likelihood) {
            result.assign({k});
            max_likelihood = curr;
        }
    }
    if (result.empty()) {
        result.resize(genotype.ploidy());
        std::iota(std::begin(result), std::end(result), 0);
    }
}

template <typename Map, typename Aligned, typename Ambiguous>
void calculate_support(Map& result,
                       const Genotype<Haplotype>& genotype,
                       const std::vector<Aligned>& reads,
                       const std::vector<double>& log_priors,
                       const HaplotypeLikelihoods& likelihoods,
                       boost::optional<Ambiguous&> ambiguous,
                       const AssignmentConfig& config)
{
    std::vector<unsigned> top {};
    top.reserve(genotype.ploidy());
    std::vector<std::shared_ptr<Haplotype>> haplotype_ptrs {};
    if (config.ambiguous_record != AssignmentConfig::AmbiguousRecord::read_only) {
        haplotype_ptrs.resize(genotype.ploidy());
    }
    for (unsigned i {0}; i < reads.size(); ++i) {
        const auto& read = reads[i];
        find_map_haplotypes(genotype, i, likelihoods, log_priors, top);
        if (top.size() == 1) {
            result[genotype[top.front()]].push_back(read);
        } else {
            using UA = AssignmentConfig::AmbiguousAction;
            switch (config.ambiguous_action) {
                case UA::first:
                    result[genotype[top.front()]].push_back(read);
                    break;
                case UA::all: {
                    for (auto idx : top) result[genotype[idx]].push_back(read);
                    break;
                }
                case UA::random: {
                    result[genotype[random_select(top)]].push_back(read);
                    break;
                }
                case UA::drop:
                default:
                    break;
            }
            if (ambiguous) {
                ambiguous->emplace_back(read);
                if (config.ambiguous_record == AssignmentConfig::AmbiguousRecord::haplotypes
                    || (config.ambiguous_record == AssignmentConfig::AmbiguousRecord::haplotypes_if_three_or_more_options && top.size() >= 3)) {
                    ambiguous->back().haplotypes.emplace();
                    ambiguous->back().haplotypes->reserve(top.size());
                    for (auto idx : top) {
                        if (!haplotype_ptrs[idx]) haplotype_ptrs[idx] = std::make_shared<Haplotype>(genotype[idx]);
                        ambiguous->back().haplotypes->push_back(haplotype_ptrs[idx]);
                    }
                }
            }
        }
        top.clear();
    }
}

auto calculate_support(const Genotype<Haplotype>& genotypes,
                       const std::vector<AlignedRead>& reads,
                       const std::vector<double>& log_priors,
                       const HaplotypeLikelihoods& likelihoods,
                       boost::optional<AmbiguousReadList&> ambiguous,
                       const AssignmentConfig& config)
{
    HaplotypeSupportMap result {};
    calculate_support(result, genotypes, reads, log_priors, likelihoods, ambiguous, config);
    return result;
}

auto calculate_support(const Genotype<Haplotype>& genotype,
                       const std::vector<AlignedTemplate>& reads,
                       const std::vector<double>& log_priors,
                       const HaplotypeLikelihoods& likelihoods,
                       boost::optional<AmbiguousTemplateList&> ambiguous,
                       const AssignmentConfig& config)
{
    HaplotypeTemplateSupportMap result {};
    calculate_support(result, genotype, reads, log_priors, likelihoods, ambiguous, config);
    return result;
}

template <typename MappableTp>
GenomicRegion::Size estimate_max_indel_size_helper(const MappableTp& mappable)
{
    const auto p = std::minmax({region_size(mappable), static_cast<GenomicRegion::Size>(sequence_size(mappable))});
    return p.second - p.first;
}

GenomicRegion::Size estimate_max_indel_size_helper(const AlignedTemplate& reads)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), GenomicRegion::Size {0},
                           [] (auto curr, const auto& read) { return curr + estimate_max_indel_size_helper(read); });
}

template <typename Range>
auto estimate_max_indel_size(const Range& mappables)
{
    GenomicRegion::Size result {0};
    for (const auto& mappable : mappables) {
        result = std::max(result, estimate_max_indel_size_helper(mappable));
    }
    return result;
}

auto compute_read_hashes(const std::vector<AlignedRead>& reads)
{
    static constexpr unsigned char mapperKmerSize {6};
    std::vector<KmerPerfectHashes> result {};
    result.reserve(reads.size());
    std::transform(std::cbegin(reads), std::cend(reads), std::back_inserter(result),
                   [=] (const AlignedRead& read) { return compute_kmer_hashes<mapperKmerSize>(read.sequence()); });
    return result;
}

auto compute_read_hashes(const std::vector<AlignedTemplate>& templates)
{
    static constexpr unsigned char mapperKmerSize {6};
    std::vector<std::vector<KmerPerfectHashes>> result {};
    result.reserve(templates.size());
    for (const auto& reads : templates) {
        std::vector<KmerPerfectHashes> hashes {};
        hashes.reserve(reads.size());
        std::transform(std::cbegin(reads), std::cend(reads), std::back_inserter(hashes),
                       [=] (const AlignedRead& read) { return compute_kmer_hashes<mapperKmerSize>(read.sequence()); });
        result.push_back(std::move(hashes));
    }
    return result;
}

auto expand_for_alignment(const Haplotype& haplotype, const GenomicRegion& reads_region,
                          const GenomicRegion::Size indel_factor, const HaplotypeLikelihoodModel& model)
{
    const auto min_flank_pad = 2 * model.pad_requirement();
    const auto& haplotype_region = mapped_region(haplotype);
    unsigned min_lhs_expansion {min_flank_pad}, min_rhs_expansion {min_flank_pad};
    if (begins_before(reads_region, haplotype_region)) {
        min_lhs_expansion += begin_distance(reads_region, haplotype_region);
    }
    if (ends_before(haplotype_region, reads_region)) {
        min_rhs_expansion += end_distance(haplotype_region, reads_region);
    }
    const auto min_expansion = std::max(min_lhs_expansion, min_rhs_expansion) + indel_factor;
    return expand(haplotype, min_expansion);
}

auto map_query_to_target_helper(const KmerPerfectHashes& query, const KmerHashTable& target,
                                MappedIndexCounts& mapping_counts)
{
    return map_query_to_target(query, target, mapping_counts);
}

std::vector<std::vector<std::size_t>>
map_query_to_target_helper(const std::vector<KmerPerfectHashes>& queries, const KmerHashTable& target,
                           MappedIndexCounts& mapping_counts)
{
    std::vector<std::vector<std::size_t>> result {};
    result.reserve(queries.size());
    for (const auto& query : queries) {
        result.push_back(map_query_to_target(query, target, mapping_counts));
        reset_mapping_counts(mapping_counts);
    }
    return result;
}

template <typename Container>
auto calculate_likelihoods(const Genotype<Haplotype>& genotype,
                           const Container& reads,
                           HaplotypeLikelihoodModel& model)
{
    const auto reads_region = encompassing_region(reads);
    const auto read_hashes = compute_read_hashes(reads);
    static constexpr unsigned char mapperKmerSize {6};
    auto haplotype_hashes = init_kmer_hash_table<mapperKmerSize>();
    HaplotypeLikelihoods result {};
    result.reserve(genotype.ploidy());
    const auto indel_factor = estimate_max_indel_size(genotype) + estimate_max_indel_size(reads);
    for (const auto& haplotype : genotype) {
        const auto expanded_haplotype = expand_for_alignment(haplotype, reads_region, indel_factor, model);
        populate_kmer_hash_table<mapperKmerSize>(expanded_haplotype.sequence(), haplotype_hashes);
        auto haplotype_mapping_counts = init_mapping_counts(haplotype_hashes);
        model.reset(expanded_haplotype);
        std::vector<double> likelihoods(reads.size());
        std::transform(std::cbegin(reads), std::cend(reads), std::cbegin(read_hashes), std::begin(likelihoods),
                       [&] (const auto& read, const auto& read_hash) {
                           auto mapping_positions = map_query_to_target_helper(read_hash, haplotype_hashes, haplotype_mapping_counts);
                           reset_mapping_counts(haplotype_mapping_counts);
                           return model.evaluate(read, mapping_positions);
                       });
        clear_kmer_hash_table(haplotype_hashes);
        result.push_back(std::move(likelihoods));
    }
    return result;
}

template <typename ReadType, typename AmbiguousReadListType>
auto
compute_haplotype_support_helper2(const Genotype<Haplotype>& genotype,
                                  const std::vector<ReadType>& reads,
                                  const HaplotypeProbabilityMap& log_priors,
                                  HaplotypeLikelihoodModel model,
                                  boost::optional<AmbiguousReadListType&> ambiguous,
                                  AssignmentConfig config)
{
    assert(genotype.ploidy() > 1);
    const auto priors = get_priors(genotype, log_priors);
    const auto likelihoods = calculate_likelihoods(genotype, reads, model);
    return calculate_support(genotype, reads, priors, likelihoods, ambiguous, config);
}

template <typename ReadType, typename AmbiguousReadListType>
auto
compute_haplotype_support_helper(const Genotype<Haplotype>& genotype,
                                 const std::vector<ReadType>& reads,
                                 const HaplotypeProbabilityMap& log_priors,
                                 HaplotypeLikelihoodModel model,
                                 boost::optional<AmbiguousReadListType&> ambiguous,
                                 AssignmentConfig config)
{
    if (is_max_zygosity(genotype)) {
        return compute_haplotype_support_helper2(genotype, reads, log_priors, std::move(model), ambiguous, std::move(config));
    } else {
        return compute_haplotype_support_helper2(collapse(genotype), reads, log_priors, std::move(model), ambiguous, std::move(config));
    }
}

} // namespace

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          const HaplotypeProbabilityMap& log_priors,
                          HaplotypeLikelihoodModel model,
                          boost::optional<AmbiguousReadList&> ambiguous,
                          AssignmentConfig config)
{
    if (!reads.empty()) {
        if (is_heterozygous(genotype)) {
            return compute_haplotype_support_helper(genotype, reads, log_priors, std::move(model), ambiguous, std::move(config));
        } else if (config.ambiguous_action != AssignmentConfig::AmbiguousAction::drop) {
            HaplotypeSupportMap result {};
            result.emplace(genotype[0], reads);
            return result;
        }
    }
    return {};
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          AmbiguousReadList& ambiguous,
                          const HaplotypeProbabilityMap& log_priors,
                          HaplotypeLikelihoodModel model,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, log_priors, model, ambiguous, config);
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          HaplotypeLikelihoodModel model,
                          AmbiguousReadList& ambiguous,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, ambiguous, {}, model, config);
}

static HaplotypeLikelihoodModel make_default_haplotype_likelihood_model()
{
    HaplotypeLikelihoodModel::Config config {};
    config.max_indel_error = 8;
    config.use_flank_state = false;
    config.use_mapping_quality = false;
    return {config};
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          AmbiguousReadList& ambiguous,
                          const HaplotypeProbabilityMap& log_priors,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, log_priors, model, ambiguous, config);
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          AmbiguousReadList& ambiguous,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, model, ambiguous, config);
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          const HaplotypeProbabilityMap& log_priors,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, log_priors, model, boost::none, config);
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, model, config);
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          HaplotypeLikelihoodModel model,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, {}, std::move(model), boost::none, config);
}

HaplotypeSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedRead>& reads,
                          AmbiguousReadList& ambiguous,
                          HaplotypeLikelihoodModel model,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, std::move(model), ambiguous, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          const HaplotypeProbabilityMap& log_priors,
                          HaplotypeLikelihoodModel model,
                          boost::optional<AmbiguousTemplateList&> ambiguous,
                          AssignmentConfig config)
{
    if (!reads.empty()) {
        if (is_heterozygous(genotype)) {
            return compute_haplotype_support_helper(genotype, reads, log_priors, std::move(model), ambiguous, std::move(config));
        } else if (config.ambiguous_action != AssignmentConfig::AmbiguousAction::drop) {
            HaplotypeTemplateSupportMap result {};
            result.emplace(genotype[0], reads);
            return result;
        }
    }
    return {};
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          AmbiguousTemplateList& ambiguous,
                          const HaplotypeProbabilityMap& log_priors,
                          HaplotypeLikelihoodModel model,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, log_priors, model, ambiguous, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          HaplotypeLikelihoodModel model,
                          AmbiguousTemplateList& ambiguous,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, ambiguous, {}, model, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          AmbiguousTemplateList& ambiguous,
                          const HaplotypeProbabilityMap& log_priors,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, log_priors, model, ambiguous, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          AmbiguousTemplateList& ambiguous,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, model, ambiguous, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          const HaplotypeProbabilityMap& log_priors,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, log_priors, model, boost::none, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          AssignmentConfig config)
{
    auto model = make_default_haplotype_likelihood_model();
    return compute_haplotype_support(genotype, reads, model, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          HaplotypeLikelihoodModel model,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, {}, std::move(model), boost::none, config);
}

HaplotypeTemplateSupportMap
compute_haplotype_support(const Genotype<Haplotype>& genotype,
                          const std::vector<AlignedTemplate>& reads,
                          AmbiguousTemplateList& ambiguous,
                          HaplotypeLikelihoodModel model,
                          AssignmentConfig config)
{
    return compute_haplotype_support(genotype, reads, std::move(model), ambiguous, config);
}

AlleleSupportMap
compute_allele_support(const std::vector<Allele>& alleles, const HaplotypeSupportMap& haplotype_support)
{
    return compute_allele_support(alleles, haplotype_support,
                                  [] (const Haplotype& haplotype, const Allele& allele) {
                                      return haplotype.includes(allele);
                                  });
}

auto copy_included(const std::vector<Allele>& alleles, const Haplotype& haplotype)
{
    std::vector<Allele> result {};
    result.reserve(alleles.size());
    std::copy_if(std::cbegin(alleles), std::cend(alleles), std::back_inserter(result),
                [&] (const auto& allele) { return haplotype.includes(allele); });
    return result;
}

struct HaveDifferentAlleles
{
    bool operator()(const std::shared_ptr<Haplotype>& lhs, const std::shared_ptr<Haplotype>& rhs) const
    {
        const auto lhs_includes = copy_included(alleles, *lhs);
        const auto rhs_includes = copy_included(alleles, *rhs);
        return lhs_includes != rhs_includes;
    }
    HaveDifferentAlleles(const std::vector<Allele>& alleles) : alleles {alleles} {}
    const std::vector<Allele>& alleles;
};

bool have_common_alleles(const std::vector<std::shared_ptr<Haplotype>>& haplotypes, const std::vector<Allele>& alleles)
{
    return std::adjacent_find(std::cbegin(haplotypes), std::cend(haplotypes), HaveDifferentAlleles {alleles}) == std::cend(haplotypes);
}

void sort_and_merge(std::deque<AlignedReadConstReference>& src, ReadRefSupportSet& dst)
{
    std::sort(std::begin(src), std::end(src));
    auto itr = dst.insert(std::end(dst), std::begin(src), std::end(src));
    std::inplace_merge(std::begin(dst), itr, std::end(dst));
}

std::size_t
try_assign_ambiguous_reads_to_alleles(const std::vector<Allele>& alleles,
                                      const AmbiguousReadList& ambiguous_reads,
                                      AlleleSupportMap& allele_support)
{
    std::size_t num_assigned {0};
    std::unordered_map<Allele, std::deque<AlignedReadConstReference>> assigned {};
    assigned.reserve(alleles.size());
    for (const auto& ambiguous_read : ambiguous_reads) {
        if (ambiguous_read.haplotypes && have_common_alleles(*ambiguous_read.haplotypes, alleles)) {
            const auto supported_alleles = copy_included(alleles, *ambiguous_read.haplotypes->front());
            for (const auto& allele : supported_alleles) {
                if (overlaps(ambiguous_read, allele)) {
                    assigned[allele].emplace_back(ambiguous_read.read);
                }
            }
        }
    }
    for (auto& p : assigned) sort_and_merge(p.second, allele_support[p.first]);
    return num_assigned;
}

AlleleSupportMap
compute_allele_support(const std::vector<Allele>& alleles,
                       const HaplotypeSupportMap& haplotype_support,
                       const AmbiguousReadList& ambiguous_reads)
{
    auto result = compute_allele_support(alleles, haplotype_support);
    try_assign_ambiguous_reads_to_alleles(alleles, ambiguous_reads, result);
    return result;
}

} // namespace octopus


#ifndef CUTSIM_H
#define CUTSIM
#include <QObject>

#include <string>
#include <iostream>
#include <cmath>
#include <vector>

#include <boost/bind.hpp>

#include <opencamlib/point.hpp>
#include <opencamlib/triangle.hpp>
#include <opencamlib/numeric.hpp>
#include <opencamlib/octree.hpp>
#include <opencamlib/octnode.hpp>

#include <opencamlib/volume.hpp>
#include <opencamlib/marching_cubes.hpp>
#include <opencamlib/gldata.hpp>

/// a Cutsim stores an Octree stock model, uses an iso-surface extraction
/// algorithm to generate surface triangles, and communicates with
/// the corresponding GLData surface which is used for rendering
class Cutsim : public QObject {
    Q_OBJECT

public:
    // std::cout << ocl::revision() << "\n";
    // std::cout << " Experimental C++ cutting simulation.\n";
    // Octree(root_scale, max_depth, cp)
    Cutsim () {
        ocl::Point octree_center(0,0,0);
        unsigned int max_depth = 7;
        tree = new ocl::Octree(10.0, max_depth, octree_center );
        std::cout << " tree before init: " << tree->str() << "\n";

        std::cout << "running init(3) on the octree. this splits the cube three times. \n";
        std::cout << "we go from 1 cube -> 8 -> 64 -> 512 cubes \n";
        tree->init(3u);
        tree->debug=false;
        std::cout << " tree after init: " << tree->str() << "\n";
        ocl::CubeVolume cube;
        cube.side = 3.5;
        cube.calcBB();

        //ocl::PlaneVolume px_plus(true, 0u, -7);
        //ocl::PlaneVolume px_minus(false, 0u, 7);

        //ocl::PlaneVolume py_plus(true, 1u, -7);
        //ocl::PlaneVolume py_minus(false, 1u, 7);

        //ocl::PlaneVolume pz_plus(true, 2u, -7);
        //ocl::PlaneVolume pz_minus(false, 2u, 7);

        tree->diff_negative( &cube);
        //tree->diff_negative( &px_plus  );
        //tree->diff_negative( &px_minus );
        //tree->diff_negative( &py_plus  );
        //tree->diff_negative( &py_minus );
        //tree->diff_negative( &pz_plus  );
        //tree->diff_negative( &pz_minus );
        std::cout << " tree after stock-cut: " << tree->str() << "\n";
        mc = new ocl::MarchingCubes();
    }
    void setGLData(ocl::GLData* gldata) {
        // this is the GLData that corresponds to the tree
        g = gldata;
        g->setTriangles(); // mc: triangles, dual_contour: quads
        g->setPosition(0,0,0); // position offset (?used)
        g->setUsageDynamicDraw();
    }
    void updateGL() {
        // traverse the octree and update the GLData correspondingly
        ocl::Octnode* root = tree->getRoot();
        updateGL(root);
    }
    void updateGL(ocl::Octnode* current) {
        // starting at current, update the isosurface
        if ( current->isLeaf() && current->surface() && !current->valid() ) {
            // this is a leaf and a surface-node
            // std::vector<ocl::Triangle> node_tris = mc->mc_node(current);
            BOOST_FOREACH(ocl::Triangle t, mc->mc_node(current) ) {
                double r=1,gr=0,b=0;
                std::vector<unsigned int> polyIndexes;
                for (int m=0;m<3;++m) { // FOUR for quads
                    //unsigned int vertexId =  g->addVertex( t.p[m].x, t.p[m].y, t.p[m].z, r,gr,b,
                    //                        boost::bind(&ocl::Octnode::swapIndex, current, _1, _2)) ; // add vertex to GL
                    unsigned int vertexId =  g->addVertex( t.p[m].x, t.p[m].y, t.p[m].z, r,gr,b, current) ;
//                        boost::bind(&ocl::Octnode::swapIndex, current, _1, _2)) ; // add vertex to GL

                    g->setNormal( vertexId, t.n.x, t.n.y, t.n.z );
                    polyIndexes.push_back( vertexId );
                    current->addIndex( vertexId ); // associate node with vertex
                }
                g->addPolygon(polyIndexes); // add poly to GL
                current->setValid(); // isosurface is now valid for this node!
            }
        } else if ( current->isLeaf() && !current->surface() && !current->valid() ) { //leaf, but no surface
            // remove vertices, if any
            BOOST_FOREACH(unsigned int vId, current->vertexSet ) {
                g->removeVertex(vId);
            }
            // needed?? // current->clearIndex();
            current->setValid();
        }
        else {
            for (int m=0;m<8;++m) { // go deeper into tree, if !valid
                if ( current->hasChild(m) && !current->valid() ) {
                    updateGL(current->child[m]);
                }
            }
        }
    }

    void surf() {
        // run mc on all nodes
        // std::vector<Triangle> mc_node(const Octnode* node);

        tree->updateGL();
        //tris = mc->mc_tree( tree ); // this gets ALL triangles from the tree and stores them here.
        //std::cout << " mc() got " << tris.size() << " triangles\n";
    }

    //std::vector<ocl::Triangle> getTris() {
    //    return tris;
    //}

public slots:
    void cut() { // demo slot of doing a cutting operation on the tree with a volume.
        std::cout << " cut! called \n";
        ocl::SphereOCTVolume s;
        s.radius = 3;
        s.center = ocl::Point(7,7,7);
        s.calcBB();
        //std::cout << " before diff: " << tree->str() << "\n";
        tree->diff_negative( &s );
        //std::cout << " AFTER diff: " << tree->str() << "\n";

        updateGL();
    }
private:
    ocl::MarchingCubes* mc; // simplest isosurface-extraction algorithm
    std::vector<ocl::Triangle> tris; // do we need to store all tris here?? no!
    ocl::Octree* tree; // this is the stock model
    ocl::GLData* g; // this is the graphics object drawn on the screen
};

#endif


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef subclone_model_hpp
#define subclone_model_hpp

#include <vector>
#include <unordered_map>
#include <utility>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/genotype.hpp"
#include "core/types/cancer_genotype.hpp"
#include "core/models/haplotype_likelihood_array.hpp"
#include "exceptions/unimplemented_feature_error.hpp"
#include "variational_bayes_mixture_model.hpp"
#include "genotype_prior_model.hpp"
#include "cancer_genotype_prior_model.hpp"

namespace octopus { namespace model {

template <typename Genotype_, typename GenotypeIndex_, typename GenotypePriorModel_>
class SubcloneModelBase
{
public:
    struct AlgorithmParameters
    {
        unsigned max_iterations = 1000;
        double epsilon          = 0.05;
        unsigned max_seeds      = 12;
        boost::optional<MemoryFootprint> target_max_memory = boost::none;
    };

    struct Priors
    {
        using GenotypeMixturesDirichletAlphas   = std::vector<double>;
        using GenotypeMixturesDirichletAlphaMap = std::unordered_map<SampleName, GenotypeMixturesDirichletAlphas>;

        const GenotypePriorModel_& genotype_prior_model;
        GenotypeMixturesDirichletAlphaMap alphas;
    };

    struct Latents
    {
        using GenotypeMixturesDirichletAlphas   = std::vector<double>;
        using GenotypeMixturesDirichletAlphaMap = std::unordered_map<SampleName, GenotypeMixturesDirichletAlphas>;
        using ProbabilityVector                 = std::vector<double>;

        ProbabilityVector genotype_probabilities;
        GenotypeMixturesDirichletAlphaMap alphas;
    };

    struct InferredLatents
    {
        Latents posteriors;
        typename Latents::ProbabilityVector genotype_log_priors;
        double approx_log_evidence;
    };

    SubcloneModelBase() = delete;

    SubcloneModelBase(std::vector<SampleName> samples, Priors priors);
    SubcloneModelBase(std::vector<SampleName> samples, Priors priors, AlgorithmParameters parameters);

    SubcloneModelBase(const SubcloneModelBase&)            = default;
    SubcloneModelBase& operator=(const SubcloneModelBase&) = default;
    SubcloneModelBase(SubcloneModelBase&&)                 = default;
    SubcloneModelBase& operator=(SubcloneModelBase&&)      = default;

    ~SubcloneModelBase() = default;

    const Priors& priors() const noexcept;

    void prime(const std::vector<Haplotype>& haplotypes);
    void unprime() noexcept;
    bool is_primed() const noexcept;

    InferredLatents evaluate(const std::vector<Genotype_>& genotypes,
                             const HaplotypeLikelihoodArray& haplotype_likelihoods) const;

    InferredLatents evaluate(const std::vector<Genotype_>& genotypes,
                             const std::vector<GenotypeIndex_>& genotype_indices,
                             const HaplotypeLikelihoodArray& haplotype_likelihoods) const;

private:
    std::vector<SampleName> samples_;
    Priors priors_;
    AlgorithmParameters parameters_;
    const std::vector<Haplotype>* haplotypes_;
};

using SubcloneModel = SubcloneModelBase<Genotype<Haplotype>, GenotypeIndex, GenotypePriorModel>;
using SomaticSubcloneModel = SubcloneModelBase<CancerGenotype<Haplotype>, CancerGenotypeIndex, CancerGenotypePriorModel>;

template <typename G, typename GI, typename GPM>
SubcloneModelBase<G, GI, GPM>::SubcloneModelBase(std::vector<SampleName> samples, Priors priors)
: SubcloneModelBase {std::move(samples), std::move(priors), AlgorithmParameters {}}
{}

template <typename G, typename GI, typename GPM>
SubcloneModelBase<G, GI, GPM>::SubcloneModelBase(std::vector<SampleName> samples, Priors priors, AlgorithmParameters parameters)
: samples_ {std::move(samples)}
, priors_ {std::move(priors)}
, parameters_ {parameters}
{}

template <typename G, typename GI, typename GPM>
const typename SubcloneModelBase<G, GI, GPM>::Priors& SubcloneModelBase<G, GI, GPM>::priors() const noexcept
{
    return priors_;
}

template <typename G, typename GI, typename GPM>
void SubcloneModelBase<G, GI, GPM>::prime(const std::vector<Haplotype>& haplotypes)
{
    haplotypes_ = std::addressof(haplotypes);
}

template <typename G, typename GI, typename GPM>
void SubcloneModelBase<G, GI, GPM>::unprime() noexcept
{
    haplotypes_ = nullptr;
}

template <typename G, typename GI, typename GPM>
bool SubcloneModelBase<G, GI, GPM>::is_primed() const noexcept
{
    return haplotypes_;
}

namespace detail {

template <typename GI>
struct IndexData
{
    const std::vector<GI>& genotype_indices;
    const std::vector<Haplotype>* haplotypes;
};

template <typename G, typename GI, typename GPM>
auto evaluate_genotype_priors(const std::vector<G>& genotypes,
                              const typename SubcloneModelBase<G, GI, GPM>::Priors& priors,
                              const boost::optional<IndexData<GI>> index_data)
{
    if (index_data) {
        return evaluate(index_data->genotype_indices, priors.genotype_prior_model);
    } else {
        return evaluate(genotypes, priors.genotype_prior_model);
    }
}

std::vector<LogProbabilityVector>
generate_seeds(const std::vector<SampleName>& samples,
               const std::vector<Genotype<Haplotype>>& genotypes,
               const LogProbabilityVector& genotype_log_priors,
               const HaplotypeLikelihoodArray& haplotype_log_likelihoods,
               const SubcloneModel::Priors& priors,
               std::size_t max_seeds,
               boost::optional<IndexData<GenotypeIndex>> index_data = boost::none);

std::vector<LogProbabilityVector>
generate_seeds(const std::vector<SampleName>& samples,
               const std::vector<CancerGenotype<Haplotype>>& genotypes,
               const LogProbabilityVector& genotype_log_priors,
               const HaplotypeLikelihoodArray& haplotype_log_likelihoods,
               const SomaticSubcloneModel::Priors& priors,
               std::size_t max_seeds,
               boost::optional<IndexData<CancerGenotypeIndex>> index_data = boost::none);

template <std::size_t K, typename G, typename GI, typename GPM>
VBAlpha<K> flatten(const typename SubcloneModelBase<G, GI, GPM>::Priors::GenotypeMixturesDirichletAlphas& alpha)
{
    VBAlpha<K> result {};
    std::copy_n(std::cbegin(alpha), K, std::begin(result));
    return result;
}

template <std::size_t K, typename G, typename GI, typename GPM>
VBAlphaVector<K> flatten(const typename SubcloneModelBase<G, GI, GPM>::Priors::GenotypeMixturesDirichletAlphaMap& alphas,
                         const std::vector<SampleName>& samples)
{
    VBAlphaVector<K> result(samples.size());
    std::transform(std::cbegin(samples), std::cend(samples), std::begin(result),
                   [&alphas] (const auto& sample) { return flatten<K, G, GI, GPM>(alphas.at(sample)); });
    return result;
}

template <std::size_t K>
VBGenotype<K>
flatten(const Genotype<Haplotype>& genotype, const SampleName& sample,
        const HaplotypeLikelihoodArray& haplotype_likelihoods)
{
    VBGenotype<K> result {};
    std::transform(std::cbegin(genotype), std::cend(genotype), std::begin(result),
                   [&sample, &haplotype_likelihoods] (const Haplotype& haplotype)
                   -> std::reference_wrapper<const VBReadLikelihoodArray::BaseType> {
                       return std::cref(haplotype_likelihoods(sample, haplotype));
                   });
    return result;
}

template <std::size_t K>
auto copy_cref(const Genotype<Haplotype>& genotype, const SampleName& sample,
               const HaplotypeLikelihoodArray& haplotype_likelihoods,
               typename VBGenotype<K>::iterator result_itr)
{
    return std::transform(std::cbegin(genotype), std::cend(genotype), result_itr,
                          [&sample, &haplotype_likelihoods] (const Haplotype& haplotype)
                          -> std::reference_wrapper<const VBReadLikelihoodArray::BaseType> {
                              return std::cref(haplotype_likelihoods(sample, haplotype));
                          });
}

template <std::size_t K>
VBGenotype<K>
flatten(const CancerGenotype<Haplotype>& genotype, const SampleName& sample,
        const HaplotypeLikelihoodArray& haplotype_likelihoods)
{
    VBGenotype<K> result {};
    assert(genotype.ploidy() == K);
    auto itr = copy_cref<K>(genotype.germline(), sample, haplotype_likelihoods, std::begin(result));
    copy_cref<K>(genotype.somatic(), sample, haplotype_likelihoods, itr);
    return result;
}

template <std::size_t K, typename G>
VBGenotypeVector<K>
flatten(const std::vector<G>& genotypes, const SampleName& sample,
        const HaplotypeLikelihoodArray& haplotype_likelihoods)
{
    VBGenotypeVector<K> result(genotypes.size());
    std::transform(std::cbegin(genotypes), std::cend(genotypes), std::begin(result),
                   [&sample, &haplotype_likelihoods] (const auto& genotype) {
                       return flatten<K>(genotype, sample, haplotype_likelihoods);
                   });
    return result;
}

template <std::size_t K, typename G>
VBReadLikelihoodMatrix<K>
flatten(const std::vector<G>& genotypes,
        const std::vector<SampleName>& samples,
        const HaplotypeLikelihoodArray& haplotype_likelihoods)
{
    VBReadLikelihoodMatrix<K> result {};
    result.reserve(samples.size());
    std::transform(std::cbegin(samples), std::cend(samples), std::back_inserter(result),
                   [&genotypes, &haplotype_likelihoods] (const auto& sample) {
                       return flatten<K>(genotypes, sample, haplotype_likelihoods);
                   });
    return result;
}

template <std::size_t K, typename G, typename GI, typename GPM>
auto expand(VBAlpha<K>& alpha)
{
    return typename SubcloneModelBase<G, GI, GPM>::Latents::GenotypeMixturesDirichletAlphas(std::begin(alpha), std::end(alpha));
}

template <std::size_t K, typename G, typename GI, typename GPM>
auto expand(const std::vector<SampleName>& samples, VBAlphaVector<K>&& alphas)
{
    typename SubcloneModelBase<G, GI, GPM>::Latents::GenotypeMixturesDirichletAlphaMap result {};
    std::transform(std::cbegin(samples), std::cend(samples), std::begin(alphas),
                   std::inserter(result, std::begin(result)),
                   [] (const auto& sample, auto&& vb_alpha) {
                       return std::make_pair(sample, expand<K, G, GI, GPM>(vb_alpha));
                   });
    return result;
}

template <std::size_t K, typename G, typename GI, typename GPM>
typename SubcloneModelBase<G, GI, GPM>::InferredLatents
expand(const std::vector<SampleName>& samples, VBLatents<K>&& inferred_latents, LogProbabilityVector genotype_log_priors, double evidence)
{
    typename SubcloneModelBase<G, GI, GPM>::Latents posterior_latents {std::move(inferred_latents.genotype_posteriors),
                                                                       expand<K, G, GI, GPM>(samples, std::move(inferred_latents.alphas))};
    return {std::move(posterior_latents), std::move(genotype_log_priors), evidence};
}

template <std::size_t K, typename G, typename GI, typename GPM>
typename SubcloneModelBase<G, GI, GPM>::InferredLatents
run_variational_bayes_helper(const std::vector<SampleName>& samples,
                             const std::vector<G>& genotypes,
                             const typename SubcloneModelBase<G, GI, GPM>::Priors::GenotypeMixturesDirichletAlphaMap& prior_alphas,
                             const LogProbabilityVector& genotype_log_priors,
                             const HaplotypeLikelihoodArray& haplotype_log_likelihoods,
                             const typename SubcloneModelBase<G, GI, GPM>::AlgorithmParameters& params,
                             std::vector<LogProbabilityVector>&& seeds)
{
    VariationalBayesParameters vb_params {params.epsilon, params.max_iterations};
    if (params.target_max_memory) {
        const auto estimated_memory_default = estimate_memory_requirement<K>(samples, haplotype_log_likelihoods, genotypes.size(), vb_params);
        if (estimated_memory_default > *params.target_max_memory) {
            vb_params.save_memory = true;
        }
    }
    const auto vb_prior_alphas = flatten<K, G, GI, GPM>(prior_alphas, samples);
    const auto log_likelihoods = flatten<K>(genotypes, samples, haplotype_log_likelihoods);
    auto p = octopus::model::run_variational_bayes(vb_prior_alphas, genotype_log_priors, log_likelihoods, vb_params, std::move(seeds));
    return expand<K, G, GI, GPM>(samples, std::move(p.first), std::move(genotype_log_priors), p.second);
}

template <typename G, typename GI, typename GPM>
typename SubcloneModelBase<G, GI, GPM>::InferredLatents
run_variational_bayes_helper(const std::vector<SampleName>& samples,
                             const std::vector<G>& genotypes,
                             const typename SubcloneModelBase<G, GI, GPM>::Priors::GenotypeMixturesDirichletAlphaMap& prior_alphas,
                             LogProbabilityVector genotype_log_priors,
                             const HaplotypeLikelihoodArray& haplotype_log_likelihoods,
                             const typename SubcloneModelBase<G, GI, GPM>::AlgorithmParameters& params,
                             std::vector<LogProbabilityVector>&& seeds)
{
    using std::move;
    switch (genotypes.front().ploidy()) {
        case 1: return run_variational_bayes_helper<1, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 2: return run_variational_bayes_helper<2, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 3: return run_variational_bayes_helper<3, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 4: return run_variational_bayes_helper<4, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 5: return run_variational_bayes_helper<5, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 6: return run_variational_bayes_helper<6, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 7: return run_variational_bayes_helper<7, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 8: return run_variational_bayes_helper<8, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 9: return run_variational_bayes_helper<9, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                   haplotype_log_likelihoods, params, move(seeds));
        case 10: return run_variational_bayes_helper<10, G, GI, GPM>(samples, genotypes, prior_alphas, move(genotype_log_priors),
                                                                     haplotype_log_likelihoods, params, move(seeds));
        default: throw UnimplementedFeatureError {"ploidies above 10", "SubcloneModel"};
    }
}

template <typename G, typename GI, typename GPM>
typename SubcloneModelBase<G, GI, GPM>::InferredLatents
run_variational_bayes(const std::vector<SampleName>& samples,
                      const std::vector<G>& genotypes,
                      const typename SubcloneModelBase<G, GI, GPM>::Priors& priors,
                      const HaplotypeLikelihoodArray& haplotype_log_likelihoods,
                      const typename SubcloneModelBase<G, GI, GPM>::AlgorithmParameters& params,
                      boost::optional<IndexData<GI>> index_data = boost::none)
{
    auto genotype_log_priors = evaluate_genotype_priors<G, GI, GPM>(genotypes, priors, index_data);
    auto seeds = generate_seeds(samples, genotypes, genotype_log_priors, haplotype_log_likelihoods, priors, params.max_seeds, index_data);
    return run_variational_bayes_helper<G, GI, GPM>(samples, genotypes, priors.alphas, std::move(genotype_log_priors),
                                                    haplotype_log_likelihoods, params, std::move(seeds));
}

} // namespace detail

template <typename G, typename GI, typename GPM>
typename SubcloneModelBase<G, GI, GPM>::InferredLatents
SubcloneModelBase<G, GI, GPM>::evaluate(const std::vector<G>& genotypes,
                                        const HaplotypeLikelihoodArray& haplotype_likelihoods) const
{
    assert(!genotypes.empty());
    return detail::run_variational_bayes<G, GI, GPM>(samples_, genotypes, priors_, haplotype_likelihoods, parameters_);
}

template <typename G, typename GI, typename GPM>
typename SubcloneModelBase<G, GI, GPM>::InferredLatents
SubcloneModelBase<G, GI, GPM>::evaluate(const std::vector<G>& genotypes,
                                        const std::vector<GI>& genotype_indices,
                                        const HaplotypeLikelihoodArray& haplotype_likelihoods) const
{
    assert(!genotypes.empty());
    assert(genotypes.size() == genotype_indices.size());
    const detail::IndexData<GI> index_data {genotype_indices, haplotypes_};
    return detail::run_variational_bayes<G, GI, GPM>(samples_, genotypes, priors_, haplotype_likelihoods, parameters_, index_data);
}

} // namespace model
} // namespace octopus

#endif


#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/inference/dynamicprogramming.hxx>
#include <param/dynamic_programming_param.hxx>

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

#if PY_MAJOR_VERSION == 2
static void wrap_import_array() {
    import_array();
}
#else
static void * wrap_import_array() {
    import_array();
}
#endif

template<class GM,class ACC>
void export_dynp(){
   using namespace boost::python;
   wrap_import_array();
   append_subnamespace("solver");
   // setup
   InfSetup setup;
   setup.algType     = "dynamic-programming";
   setup.guarantees  = "global optimal";
   setup.examples    = ">>> inference = opengm.inference.DynamicProgramming(gm=gm,accumulator='minimizer')\n"
                       "\n\n";
   setup.limitations = "graphical model must be a tree / must not have loops";

   // export parameter
   typedef opengm::DynamicProgramming<GM, ACC>  PyDynamicProgramming;
   exportInfParam<PyDynamicProgramming>("_DynamicProgramming");
   // export inference
   class_< PyDynamicProgramming>("_DynamicProgramming",init<const GM & >())
   .def(InfSuite<PyDynamicProgramming,false>(std::string("DynamicProgramming"),setup))
   ;
}

template void export_dynp<opengm::python::GmAdder,opengm::Minimizer>();
template void export_dynp<opengm::python::GmAdder,opengm::Maximizer>();
template void export_dynp<opengm::python::GmMultiplier,opengm::Minimizer>();
template void export_dynp<opengm::python::GmMultiplier,opengm::Maximizer>();


#include <cdrm_legged/leg_config_generator.h>
#include <cdrm_legged/leg_model.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <cdrm/cdrm.h>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <ros/ros.h>
#include <visualization_msgs/InteractiveMarkerUpdate.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

static const std::string TOPIC = "/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update";
static const std::string MARKER_NAME = "JJ:start_base_link";

class ContactGeneratorNode
{
public:
  ContactGeneratorNode()
    : nh_("move_group")
    , subscriber_(nh_.subscribe(TOPIC, 100, &ContactGeneratorNode::onMarkerUpdated, this))
    , publisher_(nh_.advertise<visualization_msgs::MarkerArray>("rviz_contacts", 0))
    , psm_(new planning_scene_monitor::PlanningSceneMonitor("robot_description"))
  {
    psm_->startSceneMonitor();

    loadCdrms();
    createLegModels();
  }

private:
  bool loadCdrms()
  {
    XmlRpc::XmlRpcValue filenames;

    if (!nh_.getParam("cdrms", filenames))
    {
      ROS_ERROR("No CDRM filenames specified to load on the parameter server");
      return false;
    }

    cdrms_.clear();

    for (auto it = filenames.begin(); it != filenames.end(); ++it)
    {
      const auto &filename = static_cast<std::string &>(it->second);
      std::unique_ptr<cdrm::Cdrm> cdrm(new cdrm::Cdrm);

      ROS_INFO_STREAM("Loading CDRM '" << it->first << "' from '" << filename.c_str() << "'...");

      if (cdrm->load(filename))
        cdrms_[it->first] = std::move(cdrm);
      else
        ROS_WARN_STREAM("Could not load CDRM '" << it->first << "'");
    }

    return true;
  }

  void createLegModels()
  {
    planning_scene_monitor::LockedPlanningSceneRW lock(psm_);
    const auto &robot_model = lock->getRobotModel();

    for (const auto &group_name : robot_model->getJointModelGroupNames())
    {
      XmlRpc::XmlRpcValue legs_value;

      if (!nh_.getParam("planner_configs/" + group_name + "/legs", legs_value))
        continue;

      for (auto it = legs_value.begin(); it != legs_value.end(); ++it)
      {
        const auto &leg_name = it->first;
        const auto *leg_group = robot_model->getJointModelGroup(leg_name);

        if (!leg_group)
        {
          ROS_WARN("Could not find leg joint model group '%s'", leg_name.c_str());
          continue;
        }

        const auto &cdrm_name = static_cast<std::string &>(it->second);
        const auto cdrm_it = cdrms_.find(cdrm_name);

        if (cdrm_it != cdrms_.end())
          leg_models_.emplace_back(robot_model, leg_group, cdrm_it->second.get());
        else
          ROS_WARN("No CDRM '%s' for leg '%s' in group '%s'", cdrm_name.c_str(), leg_name.c_str(), group_name.c_str());
      }

      ROS_INFO("Created leg models for '%s'", group_name.c_str());
    }
  }

  void onMarkerUpdated(const visualization_msgs::InteractiveMarkerUpdate::ConstPtr &msg)
  {
    for (const auto &pose : msg->poses)
    {
      if (pose.name != MARKER_NAME)
        continue;

      Eigen::Isometry3d updated_tf;
      tf::poseMsgToEigen(pose.pose, updated_tf);

      if (body_tf_.isApprox(updated_tf))
        continue;

      visualization_msgs::MarkerArray marker_array;

      planning_scene_monitor::LockedPlanningSceneRW lock(psm_);
      cdrm_legged::LegConfigGenerator generator(lock);

      for (std::size_t i = 0; i < leg_models_.size(); ++i)
      {
        const auto &model = leg_models_[i];
        const auto contacts = generator.generateLegConfigs(updated_tf, model);

        visualization_msgs::Marker marker;
        marker.header.frame_id = "odom";
        marker.header.stamp = ros::Time();
        marker.id = i;
        marker.type = visualization_msgs::Marker::SPHERE_LIST;
        marker.action = visualization_msgs::Marker::ADD;
        marker.scale.x = 0.02;
        marker.scale.y = 0.02;
        marker.scale.z = 0.02;
        marker.color.a = 1.0;
        marker.color.r = 0.5 + (0.25 * i);

        for (const auto vertex : contacts.contacts_)
        {
          const Eigen::VectorXd &q = model.cdrm_->getVertexConfig(vertex);
          const Eigen::Isometry3d contact_tf = updated_tf * model.tf_ * model.getTipTransform(q);

          geometry_msgs::Point point;
          point.x = contact_tf.translation().x();
          point.y = contact_tf.translation().y();
          point.z = contact_tf.translation().z();
          marker.points.push_back(point);
        }

        marker_array.markers.push_back(marker);
      }

      body_tf_ = updated_tf;

      publisher_.publish(marker_array);

      return;
    }
  }

  ros::NodeHandle nh_;
  ros::Subscriber subscriber_;
  ros::Publisher publisher_;
  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
  Eigen::Isometry3d body_tf_;

  std::map<std::string, std::unique_ptr<cdrm::Cdrm>> cdrms_;
  std::vector<cdrm_legged::LegModel> leg_models_;
};

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "contact_generator_node");
  ContactGeneratorNode node;
  ros::spin();
}


#include <iostream>
#include "glog/logging.h"
#include <time.h>
#include <vector>
#include <fstream>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/io/impl/vtk_lib_io.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#define _SILENCE_EXPERIMENTAL_FILESYSTEM_DEPRECATION_WARNING
#include <experimental/filesystem>
#include "ceres/ceres.h"
#include "class/data_path.h"
#include "class/data_structure.h"
#include "class/Visualize.h"
#include "class/reconstruction.h"
#include "class/feature_matching.h"
#include "class/ranking_system.h"

#define TOP_k 5
#define BRANCH_b 3

using namespace std;
using namespace Eigen;

vector<Geom> shard(SHARD_NUMBER);

pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("Pot reconstruction"));

VisSwitchVariables vis;

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* nothing)
{
	pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (nothing);
	std::string key_string = event.getKeySym();
	bool key_down = event.keyDown();
	vis.KeyEvent(key_string, key_down);
}

void IinitPairWiseProfileCheck(vector<Geom>& shard, list<LCSIndex> lcs_out)
{
	list<LCSIndex>::iterator iter = lcs_out.begin();
	//########### Pair-wise profile check
	vector<bool> pair_true_node(SHARD_NUMBER, false);
	for (; iter != lcs_out.end(); ) {
		int index = iter->trans_.index_ - 1;
		int toward = iter->trans_.toward_ - 1;
		if (shard[index].edge_line_.is_seg_base_ || shard[toward].edge_line_.is_seg_base_) {
			iter++;
			continue;
		}
		else
		{
			Matrix3d R;
			Vector3d t;
			iter->trans_.Output(R, t);
			shard[index].Move(R, t);
			vector<Geom*> geom_ptr;
			geom_ptr.push_back(&shard[index]);
			geom_ptr.push_back(&shard[toward]);
			RefineAxis(geom_ptr,
				shard[toward].edge_line_.axis_point_[0],
				shard[toward].edge_line_.axis_norm_[0],
				100, NUMBER_OF_THREAD, 0.5, true);
			vector<Geom*>().swap(geom_ptr);
			Matrix3d R_a, R_a_i;
			Vector3d t_a, t_a_i;
			AxisAlignment(shard[toward].edge_line_, R_a, t_a);
			shard[index].Move(R_a, t_a);

			vector<Vector3d> profile;
			ToCylindricalInterpolation(shard[index].sur_in_, profile, false);
			ToCylindricalInterpolation(shard[toward].sur_in_, profile, false);
			bool profile_matched = ProfileChecking(profile, 5.0, 10);	// 50 35

			//Count profile curve inlier
			int pc_inlier_A = 1, pc_inlier_B = 1;
			int c_node = toward + 1;
			pair_true_node[index] = true;
			CountPCInlier(pc_inlier_A, pair_true_node, shard, c_node, 5.0, 3.0, false);
			pair_true_node[index] = false;
			pair_true_node[toward] = true;
			c_node = index + 1;
			CountPCInlier(pc_inlier_B, pair_true_node, shard, c_node, 5.0, 3.0, false);
			pair_true_node[toward] = false;
			double weight = (pc_inlier_A + pc_inlier_B - 2) / 2 + 1;
			iter->inliner_ = iter->inliner_ * weight;
			//Finish count profile curve inlier

			R_a_i = R_a.inverse();
			t_a_i = -R_a_i * t_a;
			shard[toward].MoveWOSurface(R_a_i, t_a_i);
			Matrix3d R_i, R_i_total;
			Vector3d t_i, t_i_total;
			iter->trans_.InvOut(R_i, t_i);
			R_i_total = R_i * R_a_i;
			t_i_total = R_i * t_a_i + t_i;

			shard[index].Move(R_i_total, t_i_total);
			shard[toward].edge_line_.axis_norm_[0] = { 0, 0, 1 };
			shard[toward].edge_line_.axis_point_[0] = { 0, 0, 0 };
			if (profile_matched)
				iter++;
			else if (!profile_matched) {
				iter = lcs_out.erase(iter);
			}
		}
	}
}

int main(int argc, char** argv)
{
	//#################### PCL viewer setting ####################//
	double calculation_time(0);
	int s_time(0), e_time(0);

	int step_size = shard.size();
	if (argv[1] != NULL) {
		step_size = std::atoi(argv[1]);
	}
	//#################### PCL viewer setting ####################//
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

	// Register keyboard callback :
	viewer->registerKeyboardCallback(&keyboardEventOccurred, (void*)viewer.get());

	cout << "#################### Pottery Data load ####################" << endl;
	//#################### Pottery Data load ####################//
	int max_breakline_points(0);
	for (int i = 0; i < SHARD_NUMBER; i++) {
		shard[i].edge_line_.ReadAxis(axis_path[i]);

		// Consider multi-axis shards at the same time
		if(shard_on_off[i])	{
			shard[i].edge_line_.ReadPCDFileWithInfo(file_path[i]);
			shard[i].edge_line_.CalculateLineNormal();
			int breakline_points = shard[i].edge_line_.point_.cols();
			max_breakline_points = max(max_breakline_points, breakline_points);
			shard[i].LoadSurface(surface_in[i], surface_out[i]);
			shard[i].is_matching_ = true;
		}
	}

	cout << "#################### Save initial state ####################" << endl;
	//#################### Save initial state ####################//
	vector<Visualize> pc_origin(SHARD_NUMBER);
	for (int i = 0; i < SHARD_NUMBER; i++) {
		if (shard_on_off[i]) {
			std::string pointName = "origin_" + std::to_string(i + 1);
			pc_origin[i].MakePointCloud(shard[i].edge_line_.point_, shard[i].edge_line_.normal_, pointName);
			pointName = "o_Mesh" + std::to_string(i + 1);
			pc_origin[i].MakeMesh(obj_path[i], pointName);
		}
	}

	s_time = clock();

	cout << "#################### Change Axis symmetrix to z axis ####################" << endl;
	//#################### Change Axis symmetrix to z axis ####################//
	vector<Trans> T_axis(SHARD_NUMBER);
	for (int i = 0; i < SHARD_NUMBER; i++) {
		if (shard[i].is_matching_) {
			Matrix3d R_d = Matrix3d::Identity();
			Vector3d t_d = { 0, 0, 0 };
			T_axis[i].Set(R_d, t_d, i + 1, i + 1);

			// Align symmetric axis to z-axis
			AxisAlignment(shard[i].edge_line_, R_d, t_d);
			shard[i].SurMove(R_d, t_d);

			pc_origin[i].UpdateData(viewer, shard[i].edge_line_.point_, shard[i].edge_line_.normal_);
			pc_origin[i].AddPointCloud(viewer);
			pc_origin[i].MeshTransform(R_d, t_d, viewer);
			T_axis[i].Input(R_d, t_d);		// Save transformation matrix to z-axis

			//########## Align all base fragments to same direction
			if (shard[i].edge_line_.is_seg_base_) {
				cout << "Base direction check, shard : " << i + 1 << "  ";
				double direction(0);
				int base_counter(0);
				Vector3d Z_Axis = { 0, 0, 1 };
				for (int j = 0; j < shard[i].sur_in_.normal_.cols(); j++) {
					double axis_angle = acos(Z_Axis.dot(shard[i].sur_in_.normal_.col(j)));
					if (axis_angle < 0.175 || axis_angle > 3.14159 - 0.175) {
						if (axis_angle < 1.57)
							direction++;
						else
							direction--;
					}
				}
				if (direction > 0) {
					cout << ": Flip";
					shard[i].edge_line_.axis_norm_[0] = -1 * shard[i].edge_line_.axis_norm_[0];
					AxisAlignment(shard[i].edge_line_, R_d, t_d);
					shard[i].SurMove(R_d, t_d);
					pc_origin[i].MeshTransform(R_d, t_d, viewer);
					T_axis[i].Input(R_d, t_d);
				}
				cout << endl;
			}

			CalculateFeatureAxisless(shard[i]);

			//######## Multi axis
			int num_axis = shard[i].edge_line_.axis_norm_.size();
			if (num_axis > 1) {
				for (int j = 1; j < num_axis; j++) {
					Matrix3d R_a, R_i;
					Vector3d t_a, t_i;
					AxisAlignment(shard[i].edge_line_, R_a, t_a, j);
					CalculateFeatureAxisless(shard[i], j);
					R_i = R_a.inverse();
					t_i = -R_i * t_a;
					shard[i].MoveWOSurface(R_i, t_i);
				}
			}
		}
	}

	//#################### Pottery Surface Data load ####################//
	for (int i = 0; i < SHARD_NUMBER; i++) {
		if (shard[i].is_matching_) {
			int breakline_points = shard[i].edge_line_.point_.cols();
			double suf_weight = (double)breakline_points / (double)max_breakline_points;
			int num_sur_points = (int)(100.0 * suf_weight);
			shard[i].LoadSurface(surface_in[i], surface_out[i], num_sur_points);
			Matrix3d R_d = Matrix3d::Identity();
			Vector3d t_d = { 0, 0, 0 };
			T_axis[i].Output(R_d, t_d);
			shard[i].SurMove(R_d, t_d);
		}
	}

	cout << "#################### Feature matching ####################" << endl;
	////#################### Feature matching ####################//
	list<LCSIndex> LCS_out;
	FeatureComp(shard, LCS_out, 25, MINIMUM_NUMBER, 0);
	cout << "Total number : " << LCS_out.size() << endl;

	cout << "#################### Pairwise pruning ####################" << endl;
	int exclusive_index = 3;	 // rim(index 1) or base(index 2) or fractured base(index 3)
	ExclusivelyPickEdge(LCS_out, shard, exclusive_index);

	PairwisePruning(shard, LCS_out);

	IinitPairWiseProfileCheck(shard, LCS_out);

	list<LCSIndex>::iterator iter = LCS_out.begin();
	cout << "Total number pruned : " << LCS_out.size() << endl;

	int count_move_state(0);

	cout << "#################### Incremental graph building ####################" << endl;
	//////#################### Incremental graph building ####################//
	vector<RankingSubgraph> pre_graph;
	bool on_base_reconstruction = true;
	if (LCS_out.empty())
		on_base_reconstruction = false;
	int base_counter(0);
	////#################### First pahse : Reconstruct only fractured base ####################//
	list<LCSIndex> lcs_base = LCS_out;
	cout << "#################### First pahse : Reconstruct only fractured base ####################" << endl;
	while (on_base_reconstruction) {
		base_counter++;
		cout << "Reconstruct base : " << base_counter << endl;
		RankingManager base_manager(10, 5, shard, LCS_out, step_size, path + "Graph Log", exclusive_index);
		base_manager.BuildStep();
		if (!base_manager.out_graph_.empty()) {
			pre_graph.push_back(base_manager.out_graph_[0]);
		}
		for (int i = 0; i < SHARD_NUMBER; i++) {
			if (base_manager.out_graph_[0].node_[i]) {
				if (shard[i].edge_line_.is_seg_base_)
					shard[i].edge_line_.is_seg_base_ = false;
				else
					cout << "Error in a base reconstruction process" << endl;
			}
		}
		LCS_out.clear();
		LCS_out = lcs_base;
		ExclusivelyPickEdge(LCS_out, shard, exclusive_index);
		if (LCS_out.empty())
			on_base_reconstruction = false;
	}

	////#################### Second pahse : Reconstruct all fragments ####################//
	cout << "#################### Second pahse : Reconstruct all fragments ####################" << endl;
	StateManager manager(step_size, path + "Graph Log");
	manager.InitializeWithGraph(TOP_k, BRANCH_b, shard, pre_graph);
	manager.BuildStep();

	int num_total = manager.out_state_.size();

	vector<Visualize> pc_overlap(num_total);

	e_time = clock();

	cout << "Time after data load : " << (e_time - s_time)/1000 << "sec" << endl;

	bool is_whole_shards_used = false;
	count_move_state = 0;

	iter = LCS_out.begin();
	bool is_first_state = false;
	while (!viewer->wasStopped()) {
		viewer->spinOnce(5);
		if (vis.first_) {
			for (int i = 0; i < SHARD_NUMBER; i++) {
				pc_origin[i].TurnOffData(viewer);
			}
			if (count_move_state != 0) {
				TurnoffandReturn(viewer, pc_origin, manager, count_move_state);
				count_move_state = 0;
				cout << "Go to first result" << endl;
			}
			if (count_move_state == 0 && !is_first_state) {
				VisCurrentState(viewer, pc_origin, manager, count_move_state, pc_overlap, T_axis);
				pc_overlap[count_move_state].AddPointCloud(viewer, 255, 0, 0);
				is_first_state = true;
			}
			vis.first_ = false;
		}

		else if (vis.right_) {
			is_first_state = false;
			if (count_move_state >= num_total - 1) {
				cout << "There is no more result. Please press 'spacebar' to go first result" << endl;
			}
			else {
				TurnoffandReturn(viewer, pc_origin, manager, count_move_state);

				pc_overlap[count_move_state].AddPointCloud(viewer);
				count_move_state++;
				VisCurrentState(viewer, pc_origin, manager, count_move_state, pc_overlap, T_axis);

				pc_overlap[count_move_state].AddPointCloud(viewer, 255, 0, 0);
			}
			vis.right_ = false;
		}

		else if (vis.left_) {
			if (count_move_state < 1) {
				cout << "This is first result" << endl;
				is_first_state = true;
			}
			else {
				TurnoffandReturn(viewer, pc_origin, manager, count_move_state);

				pc_overlap[count_move_state].AddPointCloud(viewer);
				count_move_state--;
				VisCurrentState(viewer, pc_origin, manager, count_move_state, pc_overlap, T_axis);

				pc_overlap[count_move_state].AddPointCloud(viewer, 255, 0, 0);
			}
			vis.left_ = false;
		}

		else if (vis.obj_) {
			ObjVisualize(viewer, pc_origin, manager, count_move_state);
			vis.obj_ = false;
		}

		else if (vis.save_) {
			string path_result = path + "Result/";
			SaveResult(pc_origin, manager, count_move_state, path_result);
			vis.save_ = false;
			cout << "#################### Result save finish : " << path_result << endl;
		}
	}
	return 0;
}


#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float64MultiArray.h>

#include <Eigen/Dense>

class CollisionDetector
{
public:
    CollisionDetector(void);

    void process(void);
    bool detect_collision(geometry_msgs::Pose, geometry_msgs::Pose);
    geometry_msgs::PoseArray get_obstacles(void);

private:
    double RADIUS;
    std::string WORLD_FRAME;
    std::string ROBOT_FRAME;
    std::string OBS_PREFIX;

    ros::NodeHandle nh;
    ros::NodeHandle local_nh;

    ros::Publisher collision_pub;

    tf::TransformListener listener;

    double min_distance;
};

CollisionDetector::CollisionDetector(void)
:local_nh("~")
{
    local_nh.param<double>("/dynamic_avoidance/RADIUS", RADIUS, {0.6});
    local_nh.param<std::string>("/dynamic_avoidance/ROBOT_FRAME", ROBOT_FRAME, {"base_link"});
    local_nh.param<std::string>("/dynamic_avoidance/WORLD_FRAME", WORLD_FRAME, {"world"});
    local_nh.param<std::string>("/dynamic_avoidance/OBSTACLES_FRAME", OBS_PREFIX, {"obs"});

    collision_pub = nh.advertise<std_msgs::Float64MultiArray>("/collision_info", 1);

    min_distance = 1e6;
}

void CollisionDetector::process(void)
{
    geometry_msgs::PoseStamped current_robot;
    current_robot.header.frame_id = WORLD_FRAME;

    ros::Rate loop_rate(10);

    std::cout << "=== collision detector ===" << std::endl;

    unsigned int collision_count = 0;

    std::map<int, bool> collision_list;

    while(ros::ok()){
        bool transformed_flag = false;
        static Eigen::Vector3d last_robot_position = Eigen::Vector3d::Zero();
        double robot_speed = 0;
        try{
            static double last_transform_time = ros::Time::now().toSec();
            tf::StampedTransform transform;
            listener.lookupTransform(WORLD_FRAME, ROBOT_FRAME, ros::Time(0), transform);
            current_robot.header.stamp = transform.stamp_;
            double transform_time = current_robot.header.stamp.toSec();
            tf::poseTFToMsg(transform, current_robot.pose);
            Eigen::Vector3d robot_position(current_robot.pose.position.x, current_robot.pose.position.y, current_robot.pose.position.z);
            double dt = transform_time - last_transform_time;
            if(dt > 0.0){
                robot_speed = (robot_position - last_robot_position).norm() / dt;
            }
            last_robot_position = robot_position;
            last_transform_time = transform_time;
            transformed_flag = true;
        }catch(tf::TransformException& ex){
            std::cout << ex.what() << std::endl;
        }
        if(transformed_flag){
            geometry_msgs::PoseArray obstacles = get_obstacles();
            int obs_num = obstacles.poses.size();
            // std::cout << "obs num:" << obs_num << std::endl;
            for(int i=0;i<obs_num;i++){
                bool is_collision = detect_collision(current_robot.pose, obstacles.poses[i]);
                if(is_collision){
                    ROS_INFO_STREAM("\033[31m" << "collision detected with obs" << std::to_string(i) << "\033[0m");
                    if(collision_list[i] == false){
                        // new collision
                        collision_count++;
                        std::cout << "\033[033m" << "collision count: " << collision_count << "\033[0m" << std::endl;
                        // std::cout << "robot speed: " << sqrt(robot_vel.linear.x * robot_vel.linear.x + robot_vel.linear.y * robot_vel.linear.y) << "[m/s]" << std::endl;
                        std::cout << "robot speed: " << robot_speed << "[m/s]" << std::endl;
                        // std::cout << robot_vel << std::endl;
                    }
                    collision_list[i] = true;
                }else{
                    collision_list[i] = false;
                }
            }
            obstacles.poses.clear();
            std_msgs::Float64MultiArray data;
            data.data.push_back(collision_count);
            data.data.push_back(min_distance);
            collision_pub.publish(data);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "collision_detector");
    CollisionDetector cd;
    cd.process();
    return 0;
}

bool CollisionDetector::detect_collision(geometry_msgs::Pose p0, geometry_msgs::Pose p1)
{
    /*
     * collision->true;
     */
    double dx = p0.position.x - p1.position.x;
    double dy = p0.position.y - p1.position.y;
    double distance = sqrt(dx*dx + dy*dy);
    if(distance < min_distance){
        min_distance = distance;
        std::cout << "min distance: " << min_distance << "[m]" << std::endl;
    }
    if(distance <= RADIUS){
        std::cout << distance << "[m]" << std::endl;
        return true;
    }
    return false;
}

geometry_msgs::PoseArray CollisionDetector::get_obstacles(void)
{
    geometry_msgs::PoseArray obstacles;
    tf::FrameGraph::Request req;
    tf::FrameGraph::Response res;
    if(listener.getFrames(req, res)){
        std::vector<std::string> obs_names;
        while(1){
            // std::cout << res.dot_graph << std::endl;
            int position = res.dot_graph.find("-> \"" + OBS_PREFIX);
            // std::cout << "position: " << position << std::endl;
            if(position != std::string::npos){
                position = res.dot_graph.find(OBS_PREFIX);
                res.dot_graph.erase(0, position);
                int double_quotation_pos = res.dot_graph.find("\"");
                std::string obs_name = res.dot_graph.substr(0, double_quotation_pos);
                res.dot_graph.erase(0, double_quotation_pos);
                // std::cout << obs_name << std::endl;
                obs_names.emplace_back(obs_name);
            }else{
                // std::cout << OBS_PREFIX << " was not found" << std::endl;
                break;
            }
        }
        obstacles.header.frame_id = WORLD_FRAME;
        obstacles.header.stamp = ros::Time::now();
        for(const auto& obs_name : obs_names){
            try{
                tf::StampedTransform transform;
                listener.lookupTransform(WORLD_FRAME, obs_name, ros::Time(0), transform);
                obstacles.header.stamp = transform.stamp_;
                geometry_msgs::Pose pose;
                pose.position.x = transform.getOrigin().x();
                pose.position.y = transform.getOrigin().y();
                pose.position.z = transform.getOrigin().z();
                tf::quaternionTFToMsg(transform.getRotation(), pose.orientation);
                obstacles.poses.push_back(pose);
            }catch(tf::TransformException& ex){
                std::cout << ex.what() << std::endl;
            }
        }
    }else{
        std::cout << "cannot get frames" << std::endl;
    }
    return obstacles;
}