/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 2 of the License, or
    (at your option) any later version.

    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 General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#if defined(USE_SHADERS)
#include <assert.h>
#include <math.h>
#include <glm/gtc/type_precision.hpp>

#include <boost/array.hpp>

#include <vector>

#include "graphics.hpp"
#include "asserts.hpp"
#include "color_utils.hpp"
#include "draw_primitive.hpp"
#include "foreach.hpp"
#include "geometry.hpp"
#include "gles2.hpp"
#include "level.hpp"
#include "raster.hpp"
#include "shaders.hpp"
#include "texture.hpp"

namespace graphics
{

using namespace gles2;

namespace
{

class rect_primitive : public draw_primitive
{
public:
	explicit rect_primitive(const variant& v);
private:
	void init();

	void handle_draw() const;
#ifdef USE_ISOMAP
	void handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const;
#endif

	variant get_value(const std::string& key) const;
	void set_value(const std::string& key, const variant& value);

	rect area_;
	graphics::color color_;
	shader_program_ptr shader_;
	mutable std::vector<GLfloat> varray_;
};

rect_primitive::rect_primitive(const variant& v)
	: draw_primitive(v), area_(v["area"]), color_(v["color"]),
     shader_(gles2::get_simple_shader())
{
	if(v.has_key("shader")) {
		shader_.reset(new shader_program(v["shader"].as_string()));
	}

	init();
}

void rect_primitive::init()
{
	varray_.clear();
	varray_.push_back(area_.x());
	varray_.push_back(area_.y());
	varray_.push_back(area_.x2());
	varray_.push_back(area_.y());
	varray_.push_back(area_.x());
	varray_.push_back(area_.y2());
	varray_.push_back(area_.x2());
	varray_.push_back(area_.y2());
}

void rect_primitive::handle_draw() const
{
	color_.set_as_current_color();

	gles2::manager gles2_manager(shader_);
	gles2::active_shader()->prepare_draw();
	gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, 0, 0, &varray_.front());
	glDrawArrays(GL_TRIANGLE_STRIP, 0, varray_.size()/2);

	glColor4f(1.0, 1.0, 1.0, 1.0);
}

#ifdef USE_ISOMAP
void rect_primitive::handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
{
}
#endif

variant rect_primitive::get_value(const std::string& key) const
{
	return draw_primitive::get_value(key);
}

void rect_primitive::set_value(const std::string& key, const variant& value)
{
	draw_primitive::set_value(key, value);
}

typedef boost::array<GLfloat, 2> FPoint;

class circle_primitive : public draw_primitive
{
public:
	explicit circle_primitive(const variant& v);

private:
	void init();

	void handle_draw() const;
#ifdef USE_ISOMAP
	void handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const;
#endif
	variant get_value(const std::string& key) const;
	void set_value(const std::string& key, const variant& value);

	FPoint center_;
	float radius_;
	float y_radius_;
	float stroke_width_;

	graphics::color color_;
	graphics::color stroke_color_;

	shader_program_ptr shader_;

	mutable std::vector<GLfloat> varray_;
};

circle_primitive::circle_primitive(const variant& v)
   : draw_primitive(v),
     radius_(v["radius"].as_decimal().as_float()),
     y_radius_(v["y_radius"].as_decimal(decimal(radius_)).as_float()),
	 stroke_width_(0.0),
     shader_(gles2::get_simple_shader())
{
	if(v.has_key("shader")) {
		shader_.reset(new shader_program(v["shader"].as_string()));
	}

	center_[0] = v["x"].as_decimal().as_float();
	center_[1] = v["y"].as_decimal().as_float();

	if(v.has_key("color")) {
		color_ = color(v["color"]);
	} else {
		color_ = color(200, 0, 0, 255);
	}

	if(v.has_key("stroke_color")) {
		stroke_color_ = color(v["stroke_color"]);
		stroke_width_ = v["stroke_width"].as_decimal().as_float();
	}

	init();
}

void circle_primitive::init()
{
	varray_.clear();
	varray_.push_back(center_[0]);
	varray_.push_back(center_[1]);
	for(double angle = 0; angle < 3.1459*2.0; angle += 0.1) {
		const double xpos = center_[0] + radius_*cos(angle);
		const double ypos = center_[1] + y_radius_*sin(angle);
		varray_.push_back(xpos);
		varray_.push_back(ypos);
	}

	//repeat the first coordinate to complete the circle.
	varray_.push_back(varray_[2]);
	varray_.push_back(varray_[3]);

}

#ifdef USE_ISOMAP
void circle_primitive::handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
{
}
#endif

void circle_primitive::handle_draw() const
{
	gles2::manager gles2_manager(shader_);

	if(color_.a() > 0) {
		color_.set_as_current_color();

		gles2::active_shader()->prepare_draw();
		gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, 0, 0, &varray_.front());
		glDrawArrays(GL_TRIANGLE_FAN, 0, varray_.size()/2);
	}

	if(stroke_color_.a() > 0) {
		glLineWidth(stroke_width_);
		stroke_color_.set_as_current_color();

		gles2::active_shader()->prepare_draw();
        gles2::active_shader()->shader()->disable_vertex_attrib(-1);
		gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, 0, 0, &varray_[2]);
        glDrawArrays(GL_LINE_LOOP, 0, (varray_.size()-2)/2);
	}

	glColor4f(1.0, 1.0, 1.0, 1.0);

}

variant circle_primitive::get_value(const std::string& key) const
{
	return variant();
}

void circle_primitive::set_value(const std::string& key, const variant& value)
{
}

class arrow_primitive : public draw_primitive
{
public:
	explicit arrow_primitive(const variant& v);

private:

	void handle_draw() const;
#ifdef USE_ISOMAP
	void handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const;
#endif

	variant get_value(const std::string& key) const;
	void set_value(const std::string& key, const variant& value);

	void set_points(const variant& points);

	void curve(const FPoint& p1, const FPoint& p2, const FPoint& p3, std::vector<FPoint>* out) const;

	std::vector<FPoint> points_;
	GLfloat granularity_;
	int arrow_head_length_;
	GLfloat arrow_head_width_;
	graphics::color color_;
	int fade_in_length_;

	GLfloat width_base_, width_head_;

	mutable std::vector<GLfloat> uvarray_;
	mutable std::vector<GLfloat> varray_;
	mutable std::vector<unsigned char> carray_;

	texture texture_;
	GLfloat texture_scale_;

	void calculate_draw_arrays() const;
};

arrow_primitive::arrow_primitive(const variant& v)
  : draw_primitive(v),
    granularity_(v["granularity"].as_decimal(decimal(0.005)).as_float()),
    arrow_head_length_(v["arrow_head_length"].as_int(10)),
    arrow_head_width_(v["arrow_head_width"].as_decimal(decimal(2.0)).as_float()),
	fade_in_length_(v["fade_in_length"].as_int(50)),
	width_base_(v["width_base"].as_decimal(decimal(12.0)).as_float()),
	width_head_(v["width_head"].as_decimal(decimal(5.0)).as_float())
{
	if(v.has_key("texture")) {
		texture_ = texture::get(v["texture"].as_string());
		texture_scale_ = v["texture_scale"].as_decimal(decimal(1.0)).as_float();
	}

	if(v.has_key("color")) {
		color_ = color(v["color"]);
	} else {
		color_ = color(200, 0, 0, 255);
	}

	set_points(v["points"]);
}

void arrow_primitive::calculate_draw_arrays() const
{
	if(!varray_.empty()) {
		return;
	}

	std::vector<FPoint> path;

	for(int n = 1; n < points_.size()-1; ++n) {
		std::vector<FPoint> new_path;
		curve(points_[n-1], points_[n], points_[n+1], &new_path);

		if(path.empty()) {
			path.swap(new_path);
		} else {
			assert(path.size() >= new_path.size());
			const int overlap = path.size()/2;
			for(int n = 0; n != overlap; ++n) {
				const float ratio = float(n)/float(overlap);
				FPoint& value = path[(path.size() - overlap) + n];
				FPoint new_value = new_path[n];
				value[0] = value[0]*(1.0-ratio) + new_value[0]*ratio;
				value[1] = value[1]*(1.0-ratio) + new_value[1]*ratio;
			}

			path.insert(path.end(), new_path.begin() + overlap, new_path.end());
		}
	}

	const GLfloat PathLength = path.size()-1;

	std::vector<FPoint> left_path, right_path;
	for(int n = 0; n < path.size()-1; ++n) {
		const FPoint& p = path[n];
		const FPoint& next = path[n+1];

		FPoint direction;
		for(int m = 0; m != 2; ++m) {
			direction[m] = next[m] - p[m];
		}

		const GLfloat vector_length = sqrt(direction[0]*direction[0] + direction[1]*direction[1]);
		if(vector_length == 0.0) {
			continue;
		}

		FPoint unit_direction;
		for(int m = 0; m != 2; ++m) {
			unit_direction[m] = direction[m]/vector_length;
		}

		FPoint normal_direction_left, normal_direction_right;
		normal_direction_left[0] = -unit_direction[1];
		normal_direction_left[1] = unit_direction[0];
		normal_direction_right[0] = unit_direction[1];
		normal_direction_right[1] = -unit_direction[0];

		const GLfloat ratio = n/PathLength;

		GLfloat arrow_width = width_base_ - (width_base_-width_head_)*ratio;

		const int time_until_end = path.size()-2 - n;
		if(time_until_end < arrow_head_length_) {
			arrow_width = arrow_head_width_*time_until_end;
		}

		FPoint left, right;
		for(int m = 0; m != 2; ++m) {
			left[m] = p[m] + normal_direction_left[m]*arrow_width;
			right[m] = p[m] + normal_direction_right[m]*arrow_width;
		}

		left_path.push_back(left);
		right_path.push_back(right);
	}

	for(int n = 0; n != left_path.size(); ++n) {
		varray_.push_back(left_path[n][0]);
		varray_.push_back(left_path[n][1]);
		varray_.push_back(right_path[n][0]);
		varray_.push_back(right_path[n][1]);

		uvarray_.push_back(n*texture_scale_);
		uvarray_.push_back(0.0);
		uvarray_.push_back(n*texture_scale_);
		uvarray_.push_back(1.0);

		for(int m = 0; m != 2; ++m) {
			carray_.push_back(color_.r());
			carray_.push_back(color_.g());
			carray_.push_back(color_.b());
			if(n < fade_in_length_) {
				carray_.push_back(int((GLfloat(color_.a())*GLfloat(n)*(255.0/GLfloat(fade_in_length_)))/255.0));
			} else {
				carray_.push_back(color_.a());
			}
		}
	}
}

#ifdef USE_ISOMAP
void arrow_primitive::handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
{
}
#endif

void arrow_primitive::handle_draw() const
{
	if(points_.size() < 3) {
		return;
	}

	calculate_draw_arrays();

	gles2::manager gles2_manager(texture_.valid() ? gles2::get_texcol_shader() : gles2::get_simple_col_shader());

	if(texture_.valid()) {
		glActiveTexture(GL_TEXTURE0);
		texture_.set_as_current_texture();
		gles2::active_shader()->shader()->texture_array(2, GL_FLOAT, GL_FALSE, 0, &uvarray_[0]);
	}

	gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, GL_FALSE, 0, &varray_[0]);
	gles2::active_shader()->shader()->color_array(4, GL_UNSIGNED_BYTE, GL_TRUE, 0, &carray_[0]);

	glDrawArrays(GL_TRIANGLE_STRIP, 0, varray_.size()/2);
}

variant arrow_primitive::get_value(const std::string& key) const
{
	if(key == "points") {
		std::vector<variant> result;
		foreach(const FPoint& p, points_) {
			std::vector<variant> pos;
			pos.push_back(variant(static_cast<int>(p[0])));
			pos.push_back(variant(static_cast<int>(p[1])));
			result.push_back(variant(&pos));
		}

		return variant(&result);
	}
	ASSERT_LOG(false, "ILLEGAL KEY IN ARROW: " << key);
	return variant();
}

void arrow_primitive::set_value(const std::string& key, const variant& value)
{
	if(key == "points") {
		set_points(value);
	} else if(key == "color") {
		color_ = graphics::color(value);
	} else if(key == "granularity") {
		granularity_ = value.as_decimal().as_float();
	} else if(key == "arrow_head_length") {
		arrow_head_length_ = value.as_int();
	} else if(key == "arrow_head_width") {
		arrow_head_width_ = value.as_decimal().as_float();
	} else if(key == "fade_in_length") {
		fade_in_length_ = value.as_int();
	} else if(key == "width_base") {
		width_base_ = value.as_decimal().as_float();
	} else if(key == "width_head") {
		width_head_ = value.as_decimal().as_float();
	} else {
		ASSERT_LOG(false, "ILLEGAL KEY IN ARROW: " << key);
	}

	varray_.clear();
	carray_.clear();
}

void arrow_primitive::set_points(const variant& points)
{
	ASSERT_LOG(points.is_list(), "arrow points is not a list: " << points.debug_location());

	points_.clear();

	for(int n = 0; n != points.num_elements(); ++n) {
		variant p = points[n];
		ASSERT_LOG(p.is_list() && p.num_elements() == 2, "arrow points in invalid format: " << points.debug_location() << " : " << p.write_json());
		FPoint point;
		point[0] = p[0].as_int();
		point[1] = p[1].as_int();
		points_.push_back(point);
	}
}

void arrow_primitive::curve(const FPoint& p0, const FPoint& p1, const FPoint& p2, std::vector<FPoint>* out) const
{
	for(float t = 0.0; t < 1.0 - granularity_; t += granularity_) {
		FPoint p;
		for(int n = 0; n != 2; ++n) {
			//formula for a bezier curve.
			p[n] = (1-t)*(1-t)*p0[n] + 2*(1-t)*t*p1[n] + t*t*p2[n];
		}

		out->push_back(p);
	}
}

class wireframe_box_primitive : public draw_primitive
{
public:
	explicit wireframe_box_primitive(const variant& v);

private:
	DECLARE_CALLABLE(wireframe_box_primitive);

	void init();

	void handle_draw() const;
#ifdef USE_ISOMAP
	void handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const;
#endif

	glm::vec3 b1_;
	glm::vec3 b2_;

	graphics::color color_;

	program_ptr shader_;

	std::vector<GLfloat> varray_;

	GLuint u_mvp_matrix_;
	GLuint a_position_;
	GLuint u_color_;

	glm::vec3 translation_;
	glm::vec3 rotation_;
	glm::vec3 scale_;
};

wireframe_box_primitive::wireframe_box_primitive(const variant& v)
	: draw_primitive(v), scale_(glm::vec3(1.0f))
{
	if(v.has_key("points")) {
		ASSERT_LOG(v["points"].is_list() && v["points"].num_elements() == 2, "'points' must be a list of two elements.");
		b1_ = variant_to_vec3(v["points"][0]);
		b2_ = variant_to_vec3(v["points"][1]);
	} else {
		ASSERT_LOG(v.has_key("point1") && v.has_key("point2"), "Must specify 'points' or 'point1' and 'point2' attributes.");
		b1_ = variant_to_vec3(v["point1"]);
		b2_ = variant_to_vec3(v["point2"]);
	}
	if(v.has_key("color")) {
		color_ = color(v["color"]);
	} else {
		color_ = color(200, 0, 0, 255);
	}
	if(v.has_key("translation")) {
		translation_ = variant_to_vec3(v["translation"]);
	}
	if(v.has_key("scale")) {
		scale_ = variant_to_vec3(v["scale"]);
	}

	if(v.has_key("shader")) {
		shader_ = shader_program::get_global(v["shader"].as_string())->shader();
	} else {
		shader_ = shader_program::get_global("line_3d")->shader();
	}
	u_mvp_matrix_ = shader_->get_fixed_uniform("mvp_matrix");
	u_color_ = shader_->get_fixed_uniform("color");
	a_position_ = shader_->get_fixed_attribute("vertex");
	ASSERT_LOG(u_mvp_matrix_ != -1, "Error getting mvp_matrix uniform");
	ASSERT_LOG(u_color_ != -1, "Error getting color uniform");
	ASSERT_LOG(a_position_ != -1, "Error getting vertex attribute");

	init();
}

void wireframe_box_primitive::init()
{
	if(b1_.x > b2_.x) {
		std::swap(b1_.x, b2_.x);
	}
	if(b1_.y > b2_.y) {
		std::swap(b1_.y, b2_.y);
	}
	if(b1_.z > b2_.z) {
		std::swap(b1_.z, b2_.z);
	}

	varray_.clear();
	varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z); varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);
	varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z); varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
	varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z); varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);

	varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z); varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
	varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z); varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
	varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z); varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);

	varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z); varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
	varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z); varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);

	varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z); varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
	varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z); varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);

	varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z); varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
	varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z); varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);
}

void wireframe_box_primitive::handle_draw() const
{
}

#ifdef USE_ISOMAP
void wireframe_box_primitive::handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
{
	shader_save_context save;
	glUseProgram(shader_->get());

	glm::mat4 model = glm::translate(glm::mat4(), translation_)
		* glm::translate(glm::mat4(), glm::vec3((b2_.x - b1_.x)/2.0f,(b2_.y - b1_.y)/2.0f,(b2_.z - b1_.z)/2.0f))
		* glm::scale(glm::mat4(), scale_)
		* glm::translate(glm::mat4(), glm::vec3((b1_.x - b2_.x)/2.0f,(b1_.y - b2_.y)/2.0f,(b1_.z - b2_.z)/2.0f));
	glm::mat4 mvp = camera->projection_mat() * camera->view_mat() * model;
	glUniformMatrix4fv(u_mvp_matrix_, 1, GL_FALSE, glm::value_ptr(mvp));

	glUniform4f(u_color_, color_.r()/255.0f, color_.g()/255.0f, color_.b()/255.0f, color_.a()/255.0f);

	glEnableVertexAttribArray(a_position_);
	glVertexAttribPointer(a_position_, 3, GL_FLOAT, GL_FALSE, 0, &varray_[0]);
	glDrawArrays(GL_LINES, 0, varray_.size()/3);
	glDisableVertexAttribArray(a_position_);
}
#endif

BEGIN_DEFINE_CALLABLE(wireframe_box_primitive, draw_primitive)
	DEFINE_FIELD(color, "[int,int,int,int]")
		return obj.color_.write();
	DEFINE_SET_FIELD_TYPE("[int,int,int,int]|string")
		obj.color_ = graphics::color(value);
	DEFINE_FIELD(points, "[[decimal,decimal,decimal],[decimal,decimal,decimal]]")
		std::vector<variant> v;
		v.push_back(vec3_to_variant(obj.b1_));
		v.push_back(vec3_to_variant(obj.b2_));
		return variant(&v);
	DEFINE_SET_FIELD
		ASSERT_LOG(value.is_list() && value.num_elements() == 2, "'points' must be a list of two elements.");
		obj.b1_ = variant_to_vec3(value[0]);
		obj.b2_ = variant_to_vec3(value[1]);
		obj.init();
	DEFINE_FIELD(point1, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.b1_);
	DEFINE_SET_FIELD
		obj.b1_ = variant_to_vec3(value);
		obj.init();
	DEFINE_FIELD(point1, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.b2_);
	DEFINE_SET_FIELD
		obj.b2_ = variant_to_vec3(value);
		obj.init();
	DEFINE_FIELD(translation, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.translation_);
	DEFINE_SET_FIELD
		obj.translation_ = variant_to_vec3(value);
	DEFINE_FIELD(scale, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.scale_);
	DEFINE_SET_FIELD
		obj.scale_ = variant_to_vec3(value);
END_DEFINE_CALLABLE(wireframe_box_primitive)

}

class box_primitive : public draw_primitive
{
public:
	explicit box_primitive(const variant& v)
		: draw_primitive(v), scale_(glm::vec3(1.0f))
	{
		if(v.has_key("points")) {
			ASSERT_LOG(v["points"].is_list() && v["points"].num_elements() == 2, "'points' must be a list of two elements.");
			b1_ = variant_to_vec3(v["points"][0]);
			b2_ = variant_to_vec3(v["points"][1]);
		} else {
			ASSERT_LOG(v.has_key("point1") && v.has_key("point2"), "Must specify 'points' or 'point1' and 'point2' attributes.");
			b1_ = variant_to_vec3(v["point1"]);
			b2_ = variant_to_vec3(v["point2"]);
		}
		if(v.has_key("color")) {
			color_ = color(v["color"]);
		} else {
			color_ = color(200, 0, 0, 255);
		}
		if(v.has_key("translation")) {
			translation_ = variant_to_vec3(v["translation"]);
		}
		if(v.has_key("scale")) {
			scale_ = variant_to_vec3(v["scale"]);
		}

		if(v.has_key("shader")) {
			shader_ = shader_program::get_global(v["shader"].as_string())->shader();
		} else {
			shader_ = shader_program::get_global("line_3d")->shader();
		}
		u_mvp_matrix_ = shader_->get_fixed_uniform("mvp_matrix");
		u_color_ = shader_->get_fixed_uniform("color");
		a_position_ = shader_->get_fixed_attribute("vertex");
		ASSERT_LOG(u_mvp_matrix_ != -1, "Error getting mvp_matrix uniform");
		ASSERT_LOG(u_color_ != -1, "Error getting color uniform");
		ASSERT_LOG(a_position_ != -1, "Error getting vertex attribute");

		init();
	}
	virtual ~box_primitive()
	{}

private:
	DECLARE_CALLABLE(box_primitive);

	void init()
	{
		if(b1_.x > b2_.x) {
			std::swap(b1_.x, b2_.x);
		}
		if(b1_.y > b2_.y) {
			std::swap(b1_.y, b2_.y);
		}
		if(b1_.z > b2_.z) {
			std::swap(b1_.z, b2_.z);
		}

		varray_.clear();
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);

		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);

		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);

		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);

		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);

		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);

		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);

		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b2_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);

		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b2_.z);
		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);

		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b1_.x); varray_.push_back(b2_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);

		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);

		varray_.push_back(b2_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b2_.z);
		varray_.push_back(b1_.x); varray_.push_back(b1_.y); varray_.push_back(b1_.z);
	}

	void handle_draw() const
	{}

#ifdef USE_ISOMAP
	void handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
	{
		shader_save_context save;
		glUseProgram(shader_->get());

		glm::mat4 model = glm::translate(glm::mat4(), translation_)
			* glm::translate(glm::mat4(), glm::vec3((b2_.x - b1_.x)/2.0f,(b2_.y - b1_.y)/2.0f,(b2_.z - b1_.z)/2.0f))
			* glm::scale(glm::mat4(), scale_)
			* glm::translate(glm::mat4(), glm::vec3((b1_.x - b2_.x)/2.0f,(b1_.y - b2_.y)/2.0f,(b1_.z - b2_.z)/2.0f));
		glm::mat4 mvp = camera->projection_mat() * camera->view_mat() * model;
		glUniformMatrix4fv(u_mvp_matrix_, 1, GL_FALSE, glm::value_ptr(mvp));

		glUniform4f(u_color_, color_.r()/255.0f, color_.g()/255.0f, color_.b()/255.0f, color_.a()/255.0f);

		glEnableVertexAttribArray(a_position_);
		glVertexAttribPointer(a_position_, 3, GL_FLOAT, GL_FALSE, 0, &varray_[0]);
		glDrawArrays(GL_TRIANGLES, 0, varray_.size()/3);
		glDisableVertexAttribArray(a_position_);
	}
#endif

	glm::vec3 b1_;
	glm::vec3 b2_;

	graphics::color color_;

	program_ptr shader_;

	std::vector<GLfloat> varray_;

	GLuint u_mvp_matrix_;
	GLuint a_position_;
	GLuint u_color_;

	glm::vec3 translation_;
	glm::vec3 rotation_;
	glm::vec3 scale_;

	box_primitive();
	box_primitive(const box_primitive&);
};

BEGIN_DEFINE_CALLABLE(box_primitive, draw_primitive)
	DEFINE_FIELD(color, "[int,int,int,int]")
		return obj.color_.write();
	DEFINE_SET_FIELD_TYPE("[int,int,int,int]|string")
		obj.color_ = graphics::color(value);
	DEFINE_FIELD(points, "[[decimal,decimal,decimal],[decimal,decimal,decimal]]")
		std::vector<variant> v;
		v.push_back(vec3_to_variant(obj.b1_));
		v.push_back(vec3_to_variant(obj.b2_));
		return variant(&v);
	DEFINE_SET_FIELD
		ASSERT_LOG(value.is_list() && value.num_elements() == 2, "'points' must be a list of two elements.");
		obj.b1_ = variant_to_vec3(value[0]);
		obj.b2_ = variant_to_vec3(value[1]);
		obj.init();
	DEFINE_FIELD(point1, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.b1_);
	DEFINE_SET_FIELD
		obj.b1_ = variant_to_vec3(value);
		obj.init();
	DEFINE_FIELD(point1, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.b2_);
	DEFINE_SET_FIELD
		obj.b2_ = variant_to_vec3(value);
		obj.init();
	DEFINE_FIELD(translation, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.translation_);
	DEFINE_SET_FIELD
		obj.translation_ = variant_to_vec3(value);
	DEFINE_FIELD(scale, "[decimal,decimal,decimal]")
		return vec3_to_variant(obj.scale_);
	DEFINE_SET_FIELD
		obj.scale_ = variant_to_vec3(value);
END_DEFINE_CALLABLE(box_primitive)

class line_primitive : public draw_primitive
{
public:
    line_primitive(const variant& node);
    ~line_primitive() {}
void handle_draw() const;
#ifdef USE_ISOMAP
void handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const;
#endif
private:
    DECLARE_CALLABLE(line_primitive);
    void init();
    shader_program_ptr shader_;
    int x1_;
    int y1_;
    int x2_;
    int y2_;
    float width_;
    graphics::color color1_;
    graphics::color color2_;
    graphics::color stroke_color_;
    bool has_stroke_;
    std::vector<glm::vec2> v1array_;
    std::vector<glm::vec2> v2array_;
    std::vector<glm::u8vec4> carray_;
    line_primitive();
    line_primitive(const line_primitive&);
    line_primitive& operator=(const line_primitive&);
};

line_primitive::line_primitive(const variant& node)
    : draw_primitive(node),
      color1_(node["color1"]),
      color2_(node["color2"]),
      width_(1.0f),
      has_stroke_(false)
{
    if(node.has_key("shader")) {
        shader_.reset(new shader_program(node["shader"].as_string()));
    } else {
        shader_ = gles2::get_simple_col_shader();
    }
    if(node.has_key("p1") && node.has_key("p2")) {
        point p1(node["p1"]);
        x1_ = p1.x;
        y1_ = p1.y;
        point p2(node["p2"]);
        x2_ = p2.x;
        y2_ = p2.y;
    } else if(node.has_key("area")) {
        rect r(node["area"]);
        x1_ = r.x();
        y1_ = r.y();
        x2_ = r.x2();
        y2_ = r.y2();
    } else if(node.has_key("x1") && node.has_key("y1") && node.has_key("x2") && node.has_key("y2")) {
        x1_ = node["x1"].as_int();
        y1_ = node["y1"].as_int();
        x2_ = node["x2"].as_int();
        y2_ = node["y2"].as_int();
    } else {
        ASSERT_LOG(false, "Nothing containing points was found, either p1/p2, area or x1/y1/x2/y2 are required.");
    }
    if(node.has_key("width")) {
        width_ = static_cast<float>(node["width"].as_decimal().as_float());
    }
    if(node.has_key("stroke_color")) {
        has_stroke_ = true;
        stroke_color_ = graphics::color(node["stroke_color"]);
    }
    init();
}

void line_primitive::init()
{
    double theta = std::atan2(static_cast<double>(y2_-y1_),static_cast<double>(x2_-x1_));
    double wx_half = width_/2.0 * std::sin(theta);
    double wy_half = width_/2.0 * std::cos(theta);

    v1array_.emplace_back(static_cast<float>(x1_ - wx_half), static_cast<float>(y1_ + wy_half));
    v1array_.emplace_back(static_cast<float>(x2_ - wx_half), static_cast<float>(y2_ + wy_half));
    v1array_.emplace_back(static_cast<float>(x1_), static_cast<float>(y1_));
    v1array_.emplace_back(static_cast<float>(x2_), static_cast<float>(y2_));
    v1array_.emplace_back(static_cast<float>(x1_ + wx_half), static_cast<float>(y1_ - wy_half));
    v1array_.emplace_back(static_cast<float>(x2_ + wx_half), static_cast<float>(y2_ - wy_half));
    carray_.emplace_back(color1_.r(), color1_.g(), color1_.b(), 0);
    carray_.emplace_back(color2_.r(), color2_.g(), color2_.b(), 0);
    carray_.emplace_back(color1_.r(), color1_.g(), color1_.b(), color1_.a());
    carray_.emplace_back(color2_.r(), color2_.g(), color2_.b(), color2_.a());
    carray_.emplace_back(color1_.r(), color1_.g(), color1_.b(), 0);
    carray_.emplace_back(color2_.r(), color2_.g(), color2_.b(), 0);
    v2array_.emplace_back(static_cast<float>(x1_ - wx_half), static_cast<float>(y1_ + wy_half));
    v2array_.emplace_back(static_cast<float>(x2_ - wx_half), static_cast<float>(y2_ + wy_half));
    v2array_.emplace_back(static_cast<float>(x2_ + wx_half), static_cast<float>(y2_ - wy_half));
    v2array_.emplace_back(static_cast<float>(x1_ + wx_half), static_cast<float>(y1_ - wy_half));
}

void line_primitive::handle_draw() const
{
    gles2::manager gles2_manager(shader_);
    gles2::active_shader()->prepare_draw();

    gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, 0, 0, &v1array_.front());
    gles2::active_shader()->shader()->color_array(4, GL_UNSIGNED_BYTE, GL_TRUE, 0, &carray_.front());
    glDrawArrays(GL_TRIANGLE_STRIP, 0, v1array_.size());
    if(has_stroke_) {
        stroke_color_.set_as_current_color();
        // hack
        gles2::active_shader()->shader()->disable_vertex_attrib(-1);
        gles2::active_shader()->shader()->vertex_array(2, GL_FLOAT, 0, 0, &v2array_.front());
        glDrawArrays(GL_LINE_LOOP, 0, v2array_.size());
        glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
    }
}

#ifdef USE_ISOMAP
void line_primitive::handle_draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
{
}
#endif

BEGIN_DEFINE_CALLABLE(line_primitive, draw_primitive)
    DEFINE_FIELD(color1, "[int,int,int,int]")
        return obj.color1_.write();
    DEFINE_SET_FIELD_TYPE("[int,int,int,int]|string")
        obj.color1_ = graphics::color(value);
    DEFINE_FIELD(color2, "[int,int,int,int]")
        return obj.color2_.write();
    DEFINE_SET_FIELD_TYPE("[int,int,int,int]|string")
        obj.color2_ = graphics::color(value);
    DEFINE_FIELD(p1, "[int,int]")
        return point(obj.x1_, obj.y1_).write();
    DEFINE_SET_FIELD
        point p1(value);
        obj.x1_ = p1.x;
        obj.y1_ = p1.y;
    DEFINE_FIELD(p2, "[int,int]")
        return point(obj.x2_, obj.y2_).write();
    DEFINE_SET_FIELD
        point p2(value);
        obj.x2_ = p2.x;
        obj.y2_ = p2.y;
END_DEFINE_CALLABLE(line_primitive)

draw_primitive_ptr draw_primitive::create(const variant& v)
{
	if(v.is_callable()) {
		draw_primitive_ptr dp = v.try_convert<draw_primitive>();
		ASSERT_LOG(dp != NULL, "Couldn't convert callable type to draw_primitive");
		return dp;
	}
	const std::string type = v["type"].as_string();
	if(type == "arrow") {
		return new arrow_primitive(v);
	} else if(type == "circle") {
		return new circle_primitive(v);
	} else if(type == "rect") {
		return new rect_primitive(v);
    } else if(type == "line") {
        return new line_primitive(v);
	} else if(type == "box") {
		return new box_primitive(v);
	} else if(type == "box_wireframe") {
		return new wireframe_box_primitive(v);
	}

	ASSERT_LOG(false, "UNKNOWN DRAW PRIMITIVE TYPE: " << v["type"].as_string());
	return draw_primitive_ptr();
}

draw_primitive::draw_primitive(const variant& v)
  : src_factor_(GL_SRC_ALPHA), dst_factor_(GL_ONE_MINUS_SRC_ALPHA)
{
	if(v.has_key("blend")) {
		const std::string blend_mode = v["blend"].as_string();
		if(blend_mode == "overwrite") {
			src_factor_ = GL_ONE;
			dst_factor_ = GL_ZERO;
		} else {
			ASSERT_LOG(false, "Unrecognized blend mode: " << blend_mode);
		}
	}
}

void draw_primitive::draw() const
{
	if(src_factor_ != GL_SRC_ALPHA || dst_factor_ != GL_ONE_MINUS_SRC_ALPHA) {
		glBlendFunc(src_factor_, dst_factor_);
		handle_draw();
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	} else {
		handle_draw();
	}
}

#ifdef USE_ISOMAP
void draw_primitive::draw(const lighting_ptr& lighting, const camera_callable_ptr& camera) const
{
	if(src_factor_ != GL_SRC_ALPHA || dst_factor_ != GL_ONE_MINUS_SRC_ALPHA) {
		glBlendFunc(src_factor_, dst_factor_);
		handle_draw(lighting, camera);
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	} else {
		handle_draw(lighting, camera);
	}
}
#endif

BEGIN_DEFINE_CALLABLE_NOBASE(draw_primitive)
	DEFINE_FIELD(blend, "string")
		if(obj.src_factor_ == GL_ONE && obj.dst_factor_ == GL_ZERO) {
			return variant("overwrite");
		}
		return variant("normal");
	DEFINE_SET_FIELD
		if(value.as_string() == "overwrite") {
			obj.src_factor_ = GL_ONE;
			obj.dst_factor_ = GL_ZERO;
		} else if(value.as_string() == "normal") {
			obj.src_factor_ = GL_SRC_ALPHA;
			obj.dst_factor_ = GL_ONE_MINUS_SRC_ALPHA;
		} else {
			ASSERT_LOG(false, "Unrecognized blend mode: " << value.as_string());
		}
END_DEFINE_CALLABLE(draw_primitive)

}
#endif


#include "body_length_detector.hpp"
#include "proxy/http/header.hpp"
#include "proxy/util/misc_strings.hpp"
#include "proxy/util/utils.hpp"
#include <boost/logic/tribool.hpp>
#include <boost/tuple/tuple.hpp>
#include <vector>

namespace proxy {
namespace http_parser {

const unsigned long long MAX_LENGTH_BEFORE_LAST_DIGIT = (ULLONG_MAX - 9) / 10;
const std::string CHUNKED = "chunked";

[[nodiscard]] boost::tuple<bool, http::body_length_representation,
                           unsigned long long>
detect_body_length(http::header_container &headers) {
  for (http::header_container::name_iterator transfer_encoding_it =
           headers.find("transfer-encoding");
       transfer_encoding_it != headers.end(); transfer_encoding_it++) {
    const http::header &h = *transfer_encoding_it;
    if (h.value.length() >= CHUNKED.length()) {
      if (h.value.compare(h.value.length() - CHUNKED.length(), CHUNKED.length(),
                          CHUNKED) == 0) {
        return boost::make_tuple(true,
                                 http::body_length_representation::chunked,
                                 0ULL /* ignored */);
      }
    }
  }

  http::header_container::name_iterator content_length_it =
      headers.find("content-length");
  if (content_length_it != headers.end()) {
    unsigned long long length = 0ULL;
    const http::header &h = *content_length_it;
    for (int i = 0; i < h.value.length(); i++) {
      if (length <= MAX_LENGTH_BEFORE_LAST_DIGIT) {
        int digit = util::misc_strings::digit_to_int_safe(h.value[i]);
        if (digit > -1) {
          length = length * 10 + digit;
        } else {
          return boost::make_tuple(
              false, http::body_length_representation::none /* ignored */,
              0ULL /* ignored */);
        }
      } else {
        return boost::make_tuple(
            false, http::body_length_representation::none /* ignored */,
            0ULL /* ignored */);
      }
    }
    return boost::make_tuple(
        true, http::body_length_representation::content_length, length);
  }

  return boost::make_tuple(true, http::body_length_representation::none,
                           0ULL /* ignored */);
}

} // namespace http_parser
} // namespace proxy


/*****************************************************************************
* Short Title: class Molecule - declaration
*
* Comments:
*
* <license text>
*****************************************************************************/

#ifndef MOLECULE_HPP_INCLUDED
#define MOLECULE_HPP_INCLUDED

#include <iostream>
#include <string>
#include <vector>
#include <valarray>
#include <list>
#include <set>
#include <boost/shared_ptr.hpp>
#include "Atom_t.hpp"
#include "DistanceTable.hpp"
#include "Matrix.hpp"
#include "Random.hpp"
#include "TraceId_t.hpp"
#include "EmbedPython.hpp"
#include "ChemicalFormula.hpp"
#include "AtomRadiiTable.hpp"

class AtomFilter_t;
class AtomCost;
class AtomOverlap;
using NS_LIGA::RandomWeighedGenerator;

enum StructureType { MOLECULE, CRYSTAL };

class Molecule
{
    public:

        // friends
        friend class AtomSequence;
        friend class AtomCost;
        friend bool operator==(const Molecule&, const Molecule&);
        friend class BondAngleFilter_t;
        friend class LoneAtomFilter_t;

        // class data
        // fit parameters
        static double tol_nbad; // tolerance of normalized badness
        static double tol_r;    // position tolerance in RelaxAtom
        static bool promotejump;
        static bool promoterelax;
        static bool demoterelax;
        static double promotefrac;
        static std::vector<AtomFilter_t*> atom_filters;

        // class methods
        static void setOutputFormat(const std::string& format);

        // data
        // unique identifier
        const long id;
        std::list<TraceId_t> trace;

        // constructors
        Molecule();
        Molecule(const Molecule& M);

        // destructor
        virtual ~Molecule()  { }

        // operators
        virtual Molecule& operator=(const Molecule&);
        virtual Molecule* copy() const;     // create a copy

        // methods - class registration and type info
        virtual StructureType type() const  { return MOLECULE; }
        virtual std::string typeStr() const { return "molecule"; }

        // methods - molecule configuration
        virtual void setDistanceTable(const DistanceTable&);
        void setDistanceTable(const std::vector<double>&);
        const DistanceTable& getDistanceTable() const;
        DistanceTable getDistanceTable();

        virtual void setDistReuse(bool);
        bool getDistReuse() const;

        double getMaxAtomRadius() const;

        // methods - fitness/badness evaluation
        virtual double cost() const;    // total normalized cost
        double costDistance() const;    // normalized distance cost
        double costOverlap() const;     // normalized atom overlap
        const double& Badness() const;  // total badness
        void IncBadness(const double& db) const;
        void DecBadness(const double& db) const;
        void ResetBadness(double b=0.0) const;
        const double& Overlap() const;  // total badness
        void IncOverlap(const double& doverlap) const;
        void DecOverlap(const double& doverlap) const;
        void ResetOverlap(double overlap=0.0) const;
        bool full() const;
        int countAtoms() const;
        virtual int countPairs() const;
        double pairsPerAtom() const;
        double pairsPerAtomInc() const;
        int getMaxAtomCount() const;
        void setChemicalFormula(const std::string&);
        void setChemicalFormula(const ChemicalFormula& formula);
        ChemicalFormula getChemicalFormula() const;
        void setAtomRadiiTable(const std::string&);
        void setAtomRadiiTable(const AtomRadiiTable& radiitable);
        const AtomRadiiTable& getAtomRadiiTable() const;
        void setSamePairRadius(double);
        const double& getSamePairRadius() const;
        void reassignPairs();       // improve assignment of distances
        virtual void recalculate() const;   // recalculate everything
        virtual AtomCost* getAtomCostCalculator() const;
        virtual AtomCost* getAtomOverlapCalculator() const;
        void setAtomCostScale(double sc);
        void setAtomOverlapScale(double sc);

        // methods - molecule operations
        virtual void Shift(const R3::Vector& drc);  // cartesian shift
        void Center();    // center w/r to the center of mass

        // atom operations
        const Atom_t& getAtom(const int cidx) const  { return *atoms[cidx]; }
        virtual AtomPtr getNearestAtom(const R3::Vector& rc) const;
        void Pop(const int cidx);
        void Pop(const std::list<int>& cidx);
        virtual void Clear();           // remove all atoms
        void AddAt(const std::string& smbl, double rx0, double ry0, double rz0);
        void AddAt(const std::string& smbl, const R3::Vector& rc);
        void Add(const Atom_t& a);      // add single external atom
        void Fix(const int cidx);       // mark atom as fixed
        int NFixed() const;             // count fixed atoms
        void RelaxAtom(const int cidx); // relax internal atom
        void RelaxExternalAtom(Atom_t* pa);
        virtual const std::pair<int*,int*>& Evolve(const int* est_triang);
        enum DegenerateFlags { NONE=0, FAST=1 };
        virtual void Degenerate(int Npop, DegenerateFlags=NONE);
        double getContactRadius(const Atom_t& a0, const Atom_t& a1) const;
        void FlipSites(int idx0, int idx1);
        void DownhillOverlapMinimization();
        void MinimizeSiteOverlap(int idx);
        std::string PickElementFromBucket() const;

        // IO functions
        boost::python::object convertToDiffPyStructure() const;
        virtual void setFromDiffPyStructure(boost::python::object);
        void ReadFile(const std::string&);  // read from existing file
        void WriteFile(const std::string&, std::string title="");
        void WriteStream(std::ostream&, std::string title="") const;
        void PrintBadness() const;      // total and per-atomic badness
        void PrintFitness();            // total and per-atomic fitness
        void CheckIntegrity() const;
        R3::Vector rxaCheckGradient(const Atom_t* pa) const;
        double rxaCheckCost(const Atom_t* pa) const;

    protected:

        // types
        typedef std::vector<Atom_t> AtomArray;
        struct TriangulationAnchor
        {
            R3::Vector B0;
            R3::Vector B1;
            R3::Vector B2;
            int count;
        };

        // class data
        static std::string output_format;

        // data
        boost::shared_ptr<DistanceTable> _distance_table;
        boost::shared_ptr<AtomRadiiTable> _atom_radii_table;
        std::vector<Atom_t*> atoms;         // atoms in the Molecule
        std::vector<Atom_t*> atoms_bucket;  // available free atoms
        std::list<Atom_t> atoms_storage;    // all atom instances
        mutable SymmetricMatrix<double> pmx_partial_costs;
        mutable SymmetricMatrix<double> pmx_used_distances;
        mutable std::set<int> free_pmx_slots;
        mutable double _badness;        // molecular badness
        mutable double _overlap;        // total atom overlap

        // methods
        void AddInternalAt(Atom_t* pa, double rx0, double ry0, double rz0);
        void AddInternalAt(Atom_t* pa, const R3::Vector& rc);
        virtual void AddInternal(Atom_t* pa);   // add atom from the storage
        virtual void addNewAtomPairs(Atom_t* pa);
        virtual void removeAtomPairs(Atom_t* pa);
        Atom_t* pickAtomFromBucket() const;
        int push_good_distances(AtomArray& vta,
                const RandomWeighedGenerator& rwg, int ntrials);
        int push_good_triangles(AtomArray& vta,
                const RandomWeighedGenerator& rwg, int ntrials);
        int push_good_pyramids(AtomArray& vta,
                const RandomWeighedGenerator& rwg, int ntrials);
        virtual const TriangulationAnchor&
            getLineAnchor(const RandomWeighedGenerator& rwg);
        virtual const TriangulationAnchor&
            getPlaneAnchor(const RandomWeighedGenerator& rwg);
        virtual const TriangulationAnchor&
            getPyramidAnchor(const RandomWeighedGenerator& rwg);
        void filter_good_atoms(AtomArray& vta,
                double evolve_range, double hi_abad);
        void filter_bucket_atoms(AtomArray& vta);
        bool check_atom_filters(Atom_t*);
        virtual void resizePairMatrices(int sz);
        virtual boost::python::object newDiffPyStructure() const;
        void recalculateOverlap() const;
        enum AddRemove { ADD = 1, REMOVE = -1 };
        void applyOverlapContributions(Atom_t* pa, AddRemove sign);
        void fetchAtomRadii();
        void checkAtomIndex(int idx);

    private:

        // class methods
        static long getUniqueId();

        // data
        bool _distreuse;
        double _samepairradius;

        // methods
        // constructor helper
        void init();
        int getPairMatrixIndex();
        void returnUsedDistances();
        void rxaCheckEval(const Atom_t*, double*, R3::Vector*) const;
};

// non-member operators
bool operator==(const Molecule& m1, const Molecule& m2);
std::ostream& operator<<(std::ostream& os, const Molecule& M);

#endif  // MOLECULE_HPP_INCLUDED


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

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iostream>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/Core/Cache.h>
#include <vw/Core/ProgressCallback.h>
#include <vw/Math/Matrix.h>
#include <vw/Image/Palette.h>
#include <vw/Image/Transform.h>
#include <vw/FileIO/DiskImageResource.h>
#include <vw/FileIO/DiskImageResourceGDAL.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/Cartography/GeoReference.h>

using namespace vw;
using namespace vw::math;
using namespace vw::cartography;

int main( int argc, char *argv[] ) {

  std::string input_filename, output_filename, copy_filename, tfw_filename;
  double north_lat=90.0, south_lat=-90.0;
  double east_lon=180.0, west_lon=-180.0;
  double proj_lat=0, proj_lon=0, proj_scale=1;
  unsigned utm_zone;
  double nudge_x=0, nudge_y=0;

  po::options_description general_options("General Options");
  general_options.add_options()
    ("output-file,o", po::value<std::string>(&output_filename)->default_value("output.tif"), "Specify the base output filename")
    ("help,h", "Display this help message");

  po::options_description projection_options("Projection Options");
  projection_options.add_options()
    ("copy", po::value<std::string>(&copy_filename), "Copy the projection from the given file")
    ("tfw", po::value<std::string>(&tfw_filename), "Create a .tfw sidecar file with the given filename rather than a full copy of the image file")
    ("north", po::value<double>(&north_lat), "The northernmost latitude in degrees")
    ("south", po::value<double>(&south_lat), "The southernmost latitude in degrees")
    ("east", po::value<double>(&east_lon), "The easternmost longitude in degrees")
    ("west", po::value<double>(&west_lon), "The westernmost longitude in degrees")
    ("sinusoidal", "Assume a sinusoidal projection")
    ("mercator", "Assume a Mercator projection")
    ("transverse-mercator", "Assume a transverse Mercator projection")
    ("orthographic", "Assume an orthographic projection")
    ("stereographic", "Assume a stereographic projection")
    ("lambert-azimuthal", "Assume a Lambert azimuthal projection")
    ("utm", po::value<unsigned>(&utm_zone), "Assume UTM projection with the given zone")
    ("proj-lat", po::value<double>(&proj_lat), "The center of projection latitude (if applicable)")
    ("proj-lon", po::value<double>(&proj_lon), "The center of projection longitude (if applicable)")
    ("proj-scale", po::value<double>(&proj_scale), "The projection scale (if applicable)")
    ("nudge-x", po::value<double>(&nudge_x), "Nudge the image, in projected coordinates")
    ("nudge-y", po::value<double>(&nudge_y), "Nudge the image, in projected coordinates")
    ("pixel-as-point", "Encode that the pixel location (0,0) is the center of the upper left hand pixel (the default, if you specify nothing, is to set the upper left hand corner of the upper left pixel as (0,0) (i.e. PixelAsArea).");

  po::options_description hidden_options("");
  hidden_options.add_options()
    ("input-file", po::value<std::string>(&input_filename));

  po::options_description options("Allowed Options");
  options.add(general_options).add(projection_options).add(hidden_options);

  po::positional_options_description p;
  p.add("input-file", -1);

  std::ostringstream usage;
  usage << "Description: Specify planetary coordinates for an image" << std::endl << std::endl;
  usage << "Usage: " << argv[0] << " [options] <filename>..." << std::endl << std::endl;
  usage << general_options << std::endl;
  usage << projection_options << std::endl;

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(options).positional(p).run(), vm );
    po::notify( vm );
  } catch(po::error &e) {
    std::cout << "An error occured while parsing command line arguments.\n";
    std::cout << "\t" << e.what() << "\n\n";
    std::cout << usage.str();
    return 1;
  }

  if( vm.count("help") ) {
    std::cout << usage.str();
    return 1;
  }

  if( vm.count("input-file") < 1 ) {
    std::cout << "Error: Must specify at least one input file!" << std::endl << std::endl;
    std::cout << usage.str();
    return 1;
  }

  GeoReference output_georef;
  output_georef.set_well_known_geogcs("WGS84");

  // Read in georeference info and compute total resolution
  bool manual = vm.count("north") || vm.count("south") || vm.count("east") || vm.count("west");

  DiskImageResourceGDAL file_resource( input_filename );
  GeoReference georef;
  if( vm.count("copy") ) {
    read_georeference( georef, copy_filename );
  } else {
    read_georeference( georef, file_resource );
  }

  if ( georef.proj4_str() == "" ) georef.set_well_known_geogcs("WGS84");
  if( manual || georef.transform() == identity_matrix<3>() ) {
    if( manual ) {
      vw_out() << "Using manual Plate Carree coordinates: ";
      georef = GeoReference(georef.datum());
    } else {
      vw_out() << "No georeferencing info found.  Assuming Plate Carree WGS84: ";
      georef = GeoReference();
      georef.set_well_known_geogcs("WGS84");
    }
    vw_out() << east_lon << " E to " << west_lon << " W, "
             << south_lat << " S to " << north_lat << " N." << std::endl;

    Matrix3x3 m;
    m(0,0) = (east_lon - west_lon) / file_resource.cols();
    m(0,2) = west_lon;
    m(1,1) = (south_lat - north_lat) / file_resource.rows();
    m(1,2) = north_lat;
    m(2,2) = 1;
    georef.set_transform( m );
    manual = true;
  }
  else if( vm.count("sinusoidal") ) georef.set_sinusoidal(proj_lon);
  else if( vm.count("mercator") ) georef.set_mercator(proj_lat,proj_lon,proj_scale);
  else if( vm.count("transverse-mercator") ) georef.set_transverse_mercator(proj_lat,proj_lon,proj_scale);
  else if( vm.count("orthographic") ) georef.set_orthographic(proj_lat,proj_lon);
  else if( vm.count("stereographic") ) georef.set_stereographic(proj_lat,proj_lon,proj_scale);
  else if( vm.count("lambert-azimuthal") ) georef.set_lambert_azimuthal(proj_lat,proj_lon);
  else if( vm.count("utm") ) georef.set_UTM( utm_zone );

  if (vm.count("pixel-as-point"))
    georef.set_pixel_interpretation(GeoReference::PixelAsPoint);
  else // Default: PixelAsArea
    georef.set_pixel_interpretation(GeoReference::PixelAsArea);

  if( vm.count("nudge-x") || vm.count("nudge-y") ) {
    Matrix3x3 m = georef.transform();
    m(0,2) += nudge_x;
    m(1,2) += nudge_y;
    georef.set_transform( m );
  }

  vw_out() << "Writing file with Proj4 String: " << georef.proj4_str() << "\n";

  // Our file readers do a better job that GDAL's of coping with large
  // image files in some cases, so we make a fresh DiskImageView rather
  // than making an ImageResourceView of the existing GDAL resource.

  if (vm.count("tfw")) {
    std::ofstream tfw_file(tfw_filename.c_str());
    tfw_file << georef.transform()(0,0) << "\n";
    tfw_file << georef.transform()(0,1) << "\n";
    tfw_file << georef.transform()(1,0) << "\n";
    tfw_file << georef.transform()(1,1) << "\n";
    if (vm.count("pixel-as-point")) {
      tfw_file << georef.transform()(0,2) << "\n";
      tfw_file << georef.transform()(1,2) << "\n";
    } else {
      tfw_file << georef.transform()(0,2) + 0.5*georef.transform()(0,0) << "\n";
      tfw_file << georef.transform()(1,2) + 0.5*georef.transform()(1,1) << "\n";
    }
    tfw_file.close();
  } else {
    TerminalProgressCallback bar( "tools.georef", "Writing:" );
    switch( file_resource.channel_type() ) {
    case VW_CHANNEL_INT16:
      switch( file_resource.pixel_format() ) {
      case VW_PIXEL_SCALAR: {
        DiskImageView<int16> input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAY: {
        DiskImageView<PixelGray<int16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAYA: {
        DiskImageView<PixelGrayA<int16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGB: {
        DiskImageView<PixelRGB<int16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGBA: {
        DiskImageView<PixelRGBA<int16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      default: {
        vw_throw( NoImplErr() << "Unsupported pixel format: " << file_resource.pixel_format() );
        break;
      }
      }
      break;
    case VW_CHANNEL_UINT16:
      switch( file_resource.pixel_format() ) {
      case VW_PIXEL_SCALAR: {
        DiskImageView<uint16> input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAY: {
        DiskImageView<PixelGray<uint16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAYA: {
        DiskImageView<PixelGrayA<uint16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGB: {
        DiskImageView<PixelRGB<uint16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGBA: {
        DiskImageView<PixelRGBA<uint16> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      default: {
        vw_throw( NoImplErr() << "Unsupported pixel format: " << file_resource.pixel_format() );
        break;
      }
      }
      break;
    case VW_CHANNEL_FLOAT32:
      switch( file_resource.pixel_format() ) {
      case VW_PIXEL_SCALAR: {
        DiskImageView<float32> input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAY: {
        DiskImageView<PixelGray<float32> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAYA: {
        DiskImageView<PixelGrayA<float32> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGB: {
        DiskImageView<PixelRGB<float32> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGBA: {
        DiskImageView<PixelRGBA<float32> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      default: {
        vw_throw( NoImplErr() << "Unsupported pixel format: " << file_resource.pixel_format() );
        break;
      }
      }
      break;
    default:
      switch( file_resource.pixel_format() ) {
      case VW_PIXEL_SCALAR: {
        DiskImageView<uint8> input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAY: {
        DiskImageView<PixelGray<uint8> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_GRAYA: {
        DiskImageView<PixelGrayA<uint8> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGB: {
        DiskImageView<PixelRGB<uint8> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      case VW_PIXEL_RGBA: {
        DiskImageView<PixelRGBA<uint8> > input_image( input_filename );
        write_georeferenced_image( output_filename, input_image, georef, bar );
        break;
      }
      default: {
        vw_throw( NoImplErr() << "Unsupported pixel format: " << file_resource.pixel_format() );
        break;
      }
      }
      break;
    }
  }
  return 0;
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_unsigned.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD( rol_
                          , (typename A0)
                          , bs::vmx_
                          , bs::pack_< bd::unsigned_<A0>, bs::vmx_>
                          , bs::pack_< bd::unsigned_<A0>, bs::vmx_>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const
      {
        BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "rol : rotation is out of range");
        return vec_rl(a0.storage(), a1.storage());
      }
   };

} } }

#endif


//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/boostorg/beast
//

#ifndef BOOST_BEAST_WEBSOCKET_DETAIL_PRNG_IPP
#define BOOST_BEAST_WEBSOCKET_DETAIL_PRNG_IPP

#include <boost/beast/websocket/detail/prng.hpp>
#include <boost/beast/core/detail/chacha.hpp>
#include <boost/beast/core/detail/pcg.hpp>
#include <atomic>
#include <cstdlib>
#include <mutex>
#include <random>

namespace boost {
namespace beast {
namespace websocket {
namespace detail {

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

std::uint32_t const*
prng_seed(std::seed_seq* ss)
{
    struct data
    {
        std::uint32_t v[8];

        explicit
        data(std::seed_seq* pss)
        {
            if(! pss)
            {
                std::random_device g;
                std::seed_seq ss{
                    g(), g(), g(), g(),
                    g(), g(), g(), g()};
                ss.generate(v, v+8);
            }
            else
            {
                pss->generate(v, v+8);
            }
        }
    };
    static data const d(ss);
    return d.v;
}

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

inline
std::uint32_t
make_nonce()
{
    static std::atomic<std::uint32_t> nonce{0};
    return ++nonce;
}

inline
beast::detail::pcg make_pcg()
{
    auto const pv = prng_seed();
    return beast::detail::pcg{
        ((static_cast<std::uint64_t>(pv[0])<<32)+pv[1]) ^
        ((static_cast<std::uint64_t>(pv[2])<<32)+pv[3]) ^
        ((static_cast<std::uint64_t>(pv[4])<<32)+pv[5]) ^
        ((static_cast<std::uint64_t>(pv[6])<<32)+pv[7]), make_nonce()};
}

#ifdef BOOST_NO_CXX11_THREAD_LOCAL

inline
std::uint32_t
secure_generate()
{
    struct generator
    {
        std::uint32_t operator()()
        {
            std::lock_guard<std::mutex> guard{mtx};
            return gen();
        }

        beast::detail::chacha<20> gen;
        std::mutex mtx;
    };
    static generator gen{beast::detail::chacha<20>{prng_seed(), make_nonce()}};
    return gen();
}

inline
std::uint32_t
fast_generate()
{
    struct generator
    {
        std::uint32_t operator()()
        {
            std::lock_guard<std::mutex> guard{mtx};
            return gen();
        }

        beast::detail::pcg gen;
        std::mutex mtx;
    };
    static generator gen{make_pcg()};
    return gen();
}

#else

inline
std::uint32_t
secure_generate()
{
    thread_local static beast::detail::chacha<20> gen{prng_seed(), make_nonce()};
    return gen();
}

inline
std::uint32_t
fast_generate()
{
    thread_local static beast::detail::pcg gen{make_pcg()};
    return gen();
}

#endif

generator
make_prng(bool secure)
{
    if (secure)
        return &secure_generate;
    else
        return &fast_generate;
}

} // detail
} // websocket
} // beast
} // boost

#endif


#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <geometry_msgs/PointStamped.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_model/robot_model.h>
#include <humanoid_catching/IKMetric.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <tf/transform_listener.h>
#include <pluginlib/class_loader.h>
#include <boost/random.hpp>

using namespace std;

class IKTester {
private:
    ros::NodeHandle nh;
    ros::NodeHandle pnh;

    auto_ptr<message_filters::Subscriber<geometry_msgs::PointStamped> > trialGoalSub;
    kinematics::KinematicsBasePtr kinematicsSolver;
    ros::Publisher ikMetricsPub;

    string endEffector;
    string arm;
    string commandTopic;

    robot_model::JointModelGroup* jointModelGroup;
    robot_state::RobotStatePtr kinematicState;
    boost::shared_ptr<pluginlib::ClassLoader<kinematics::KinematicsBase> > kinematicsLoader;

    //! rng
    boost::mt19937 rng;
public:
    IKTester() :
            pnh("~")
        {

        if (!pnh.getParam("end_effector", endEffector))
        {
            ROS_ERROR("End effector name must be specified");
        }
        else {
            ROS_INFO("Configuring for end effector: [%s]", endEffector.c_str());
        }

        if (!pnh.getParam("arm", arm))
        {
            ROS_ERROR("Arm name must be specified");
        }
        else {
            ROS_INFO("Configuring for arm: [%s]", arm.c_str());
        }

        if (!pnh.getParam("command_topic", commandTopic))
        {
            ROS_ERROR("Command topic must be specified");
        }
        else {
            ROS_INFO("Subscribing to topic: [%s]", commandTopic.c_str());
        }

        // Setup the subscriber
        trialGoalSub.reset(
                new message_filters::Subscriber<geometry_msgs::PointStamped>(nh,
                        commandTopic, 100000));
        trialGoalSub->registerCallback(boost::bind(&IKTester::goalCallback, this, _1));

        // Setup the publisher
        ikMetricsPub = nh.advertise<humanoid_catching::IKMetric>("ik_metric", 100000, true);

        robot_model_loader::RobotModelLoader robotModelLoader("robot_description");
        robot_model::RobotModelPtr kinematicModel = robotModelLoader.getModel();

        kinematicState.reset(new robot_state::RobotState(kinematicModel));
        kinematicState->setToDefaultValues();
        jointModelGroup = kinematicModel->getJointModelGroup(arm);

        kinematicsLoader.reset(new pluginlib::ClassLoader<kinematics::KinematicsBase>("moveit_core", "kinematics::KinematicsBase"));
        try {
            kinematicsSolver = kinematicsLoader->createInstance("pr2_arm_kinematics/PR2ArmKinematicsPlugin");
        } catch(pluginlib::PluginlibException& ex) //handle the class failing to load
        {
            ROS_ERROR("The plugin failed to load. Error: %s", ex.what());
            throw ex;
        }

        if(!kinematicsSolver->initialize("robot_description", arm, "/torso_lift_link", endEffector, 0.02 /* max error */)) {
            ROS_ERROR("Could not initialize solver");
        }
        else {
            ROS_INFO("Initialized solver successfully");
        }

        rng.seed(1000);
    }

private:

    void goalCallback(const geometry_msgs::PointStampedConstPtr& point) {

        ROS_DEBUG("Querying for solution");

        ros::WallTime start = ros::WallTime::now();

        // Create a pose in the default orientation.
        geometry_msgs::Pose pose;
        pose.position = point->point;
        tf::Quaternion identity = tf::createIdentityQuaternion();
        tf::quaternionTFToMsg(identity, pose.orientation);

        vector<double> jointValues(jointModelGroup->getJointModels().size());
        vector<double> solution(7);
        moveit_msgs::MoveItErrorCodes errorCode;

        unsigned int count = 0;
        for (unsigned int i = 0; i < 50; ++i) {
            for (unsigned int i = 0; i < jointValues.size(); ++i) {
                boost::uniform_real<double> range(jointModelGroup->getJointModels()[i]->getVariableBounds()[0].first,
                                                jointModelGroup->getJointModels()[i]->getVariableBounds()[0].second);
                boost::variate_generator<boost::mt19937&, boost::uniform_real<double> > getRandom(rng, range);
                jointValues[i] = getRandom();
                ROS_DEBUG("Set initial position to [%f]", jointValues[i]);
            }

            kinematicsSolver->searchPositionIK(pose, jointValues, 0.1 /* timeout */, solution, errorCode);
            if(errorCode.val == moveit_msgs::MoveItErrorCodes::SUCCESS) {
                ROS_DEBUG("Solution found");
                count++;
            }
            else {
                ROS_INFO("IK failed %i", errorCode.val);
            }
        }

        humanoid_catching::IKMetric msg;
        msg.time = ros::Duration((ros::WallTime::now() - start).toSec());
        msg.was_successful = (count > 0);
        msg.count = count;
        ikMetricsPub.publish(msg);
    }
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "ik_tester");
    IKTester ik;
    ros::spin();
    return 0;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Evolution/Systems/GrMhd/GhValenciaDivClean/FiniteDifference/MonotisedCentral.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <memory>
#include <pup.h>
#include <utility>

#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/FixedHashMap.hpp"
#include "DataStructures/Index.hpp"
#include "DataStructures/Tensor/EagerMath/DeterminantAndInverse.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "Domain/Structure/Side.hpp"
#include "Evolution/DiscontinuousGalerkin/Actions/NormalCovectorAndMagnitude.hpp"
#include "Evolution/Systems/GeneralizedHarmonic/Tags.hpp"
#include "Evolution/Systems/GrMhd/GhValenciaDivClean/FiniteDifference/ReconstructWork.tpp"
#include "Evolution/Systems/GrMhd/GhValenciaDivClean/System.hpp"
#include "Evolution/Systems/GrMhd/ValenciaDivClean/Tags.hpp"
#include "NumericalAlgorithms/FiniteDifference/MonotisedCentral.hpp"
#include "NumericalAlgorithms/FiniteDifference/Unlimited.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "PointwiseFunctions/GeneralRelativity/Lapse.hpp"
#include "PointwiseFunctions/GeneralRelativity/Shift.hpp"
#include "PointwiseFunctions/GeneralRelativity/SpatialMetric.hpp"
#include "PointwiseFunctions/GeneralRelativity/Tags.hpp"
#include "PointwiseFunctions/Hydro/EquationsOfState/EquationOfState.hpp"
#include "PointwiseFunctions/Hydro/Tags.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

namespace grmhd::GhValenciaDivClean::fd {
MonotisedCentralPrim::MonotisedCentralPrim(CkMigrateMessage* const msg)
    : Reconstructor(msg) {}

std::unique_ptr<Reconstructor> MonotisedCentralPrim::get_clone() const {
  return std::make_unique<MonotisedCentralPrim>(*this);
}

void MonotisedCentralPrim::pup(PUP::er& p) { Reconstructor::pup(p); }

// NOLINTNEXTLINE
PUP::able::PUP_ID MonotisedCentralPrim::my_PUP_ID = 0;

template <size_t ThermodynamicDim, typename TagsList>
void MonotisedCentralPrim::reconstruct(
    const gsl::not_null<std::array<Variables<TagsList>, dim>*>
        vars_on_lower_face,
    const gsl::not_null<std::array<Variables<TagsList>, dim>*>
        vars_on_upper_face,
    const Variables<hydro::grmhd_tags<DataVector>>& volume_prims,
    const Variables<typename System::variables_tag::type::tags_list>&
        volume_spacetime_and_cons_vars,
    const EquationsOfState::EquationOfState<true, ThermodynamicDim>& eos,
    const Element<dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(dim) + 1,
        std::pair<Direction<dim>, ElementId<dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<dim>, ElementId<dim>>>>& neighbor_data,
    const Mesh<dim>& subcell_mesh) const {
  reconstruct_prims_work<tmpl::list<gr::Tags::SpacetimeMetric<3>>>(
      vars_on_lower_face, vars_on_upper_face,
      [](auto upper_face_vars_ptr, auto lower_face_vars_ptr,
         const auto& volume_vars, const auto& ghost_cell_vars,
         const auto& subcell_extents, const size_t number_of_variables) {
        ::fd::reconstruction::monotised_central(
            upper_face_vars_ptr, lower_face_vars_ptr, volume_vars,
            ghost_cell_vars, subcell_extents, number_of_variables);
      },
      [](auto upper_face_vars_ptr, auto lower_face_vars_ptr,
         const auto& volume_vars, const auto& ghost_cell_vars,
         const auto& subcell_extents, const size_t number_of_variables) {
        ::fd::reconstruction::unlimited<2>(
            upper_face_vars_ptr, lower_face_vars_ptr, volume_vars,
            ghost_cell_vars, subcell_extents, number_of_variables);
      },
      [](const auto vars_on_face_ptr) {
        const auto& spacetime_metric =
            get<gr::Tags::SpacetimeMetric<3>>(*vars_on_face_ptr);
        auto& spatial_metric =
            get<gr::Tags::SpatialMetric<3>>(*vars_on_face_ptr);
        gr::spatial_metric(make_not_null(&spatial_metric), spacetime_metric);
        auto& inverse_spatial_metric =
            get<gr::Tags::InverseSpatialMetric<3>>(*vars_on_face_ptr);
        auto& sqrt_det_spatial_metric =
            get<gr::Tags::SqrtDetSpatialMetric<>>(*vars_on_face_ptr);

        determinant_and_inverse(make_not_null(&sqrt_det_spatial_metric),
                                make_not_null(&inverse_spatial_metric),
                                spatial_metric);
        get(sqrt_det_spatial_metric) = sqrt(get(sqrt_det_spatial_metric));

        auto& shift = get<gr::Tags::Shift<3>>(*vars_on_face_ptr);
        gr::shift(make_not_null(&shift), spacetime_metric,
                  inverse_spatial_metric);
        gr::lapse(make_not_null(&get<gr::Tags::Lapse<>>(*vars_on_face_ptr)),
                  shift, spacetime_metric);
      },
      volume_prims, volume_spacetime_and_cons_vars, eos, element, neighbor_data,
      subcell_mesh, ghost_zone_size());
}

template <size_t ThermodynamicDim, typename TagsList>
void MonotisedCentralPrim::reconstruct_fd_neighbor(
    const gsl::not_null<Variables<TagsList>*> vars_on_face,
    const Variables<hydro::grmhd_tags<DataVector>>& subcell_volume_prims,
    const Variables<tmpl::list<
        gr::Tags::SpacetimeMetric<3>, GeneralizedHarmonic::Tags::Phi<3>,
        GeneralizedHarmonic::Tags::Pi<3>>>& subcell_volume_spacetime_metric,
    const EquationsOfState::EquationOfState<true, ThermodynamicDim>& eos,
    const Element<dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(dim) + 1,
        std::pair<Direction<dim>, ElementId<dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<dim>, ElementId<dim>>>>& neighbor_data,
    const Mesh<dim>& subcell_mesh,
    const Direction<dim> direction_to_reconstruct) const {
  reconstruct_fd_neighbor_work(
      vars_on_face,
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor,
         const Index<dim>& subcell_extents,
         const Index<dim>& ghost_data_extents,
         const Direction<dim>& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Lower,
            ::fd::reconstruction::detail::MonotisedCentralReconstructor>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor,
         const Index<dim>& subcell_extents,
         const Index<dim>& ghost_data_extents,
         const Direction<dim>& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Lower,
            ::fd::reconstruction::detail::UnlimitedReconstructor<2>>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor,
         const Index<dim>& subcell_extents,
         const Index<dim>& ghost_data_extents,
         const Direction<dim>& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Upper,
            ::fd::reconstruction::detail::MonotisedCentralReconstructor>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor,
         const Index<dim>& subcell_extents,
         const Index<dim>& ghost_data_extents,
         const Direction<dim>& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Upper,
            ::fd::reconstruction::detail::UnlimitedReconstructor<2>>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      [](const auto vars_on_face_ptr) {
        const auto& spacetime_metric =
            get<gr::Tags::SpacetimeMetric<3>>(*vars_on_face_ptr);
        auto& spatial_metric =
            get<gr::Tags::SpatialMetric<3>>(*vars_on_face_ptr);
        gr::spatial_metric(make_not_null(&spatial_metric), spacetime_metric);
        auto& inverse_spatial_metric =
            get<gr::Tags::InverseSpatialMetric<3>>(*vars_on_face_ptr);
        auto& sqrt_det_spatial_metric =
            get<gr::Tags::SqrtDetSpatialMetric<>>(*vars_on_face_ptr);

        determinant_and_inverse(make_not_null(&sqrt_det_spatial_metric),
                                make_not_null(&inverse_spatial_metric),
                                spatial_metric);
        get(sqrt_det_spatial_metric) = sqrt(get(sqrt_det_spatial_metric));

        auto& shift = get<gr::Tags::Shift<3>>(*vars_on_face_ptr);
        gr::shift(make_not_null(&shift), spacetime_metric,
                  inverse_spatial_metric);
        gr::lapse(make_not_null(&get<gr::Tags::Lapse<>>(*vars_on_face_ptr)),
                  shift, spacetime_metric);
      },
      subcell_volume_prims, subcell_volume_spacetime_metric, eos, element,
      neighbor_data, subcell_mesh, direction_to_reconstruct, ghost_zone_size());
}

bool operator==(const MonotisedCentralPrim& /*lhs*/,
                const MonotisedCentralPrim& /*rhs*/) {
  return true;
}

bool operator!=(const MonotisedCentralPrim& lhs,
                const MonotisedCentralPrim& rhs) {
  return not(lhs == rhs);
}

#define THERMO_DIM(data) BOOST_PP_TUPLE_ELEM(0, data)
#define TAGS_LIST(data)                                                       \
  tmpl::list<                                                                 \
      gr::Tags::SpacetimeMetric<3>, GeneralizedHarmonic::Tags::Pi<3>,         \
      GeneralizedHarmonic::Tags::Phi<3>, ValenciaDivClean::Tags::TildeD,      \
      ValenciaDivClean::Tags::TildeTau,                                       \
      ValenciaDivClean::Tags::TildeS<Frame::Inertial>,                        \
      ValenciaDivClean::Tags::TildeB<Frame::Inertial>,                        \
      ValenciaDivClean::Tags::TildePhi,                                       \
      hydro::Tags::RestMassDensity<DataVector>,                               \
      hydro::Tags::SpecificInternalEnergy<DataVector>,                        \
      hydro::Tags::SpatialVelocity<DataVector, 3>,                            \
      hydro::Tags::MagneticField<DataVector, 3>,                              \
      hydro::Tags::DivergenceCleaningField<DataVector>,                       \
      hydro::Tags::LorentzFactor<DataVector>,                                 \
      hydro::Tags::Pressure<DataVector>,                                      \
      hydro::Tags::SpecificEnthalpy<DataVector>,                              \
      hydro::Tags::LorentzFactorTimesSpatialVelocity<DataVector, 3>,          \
      ::Tags::Flux<ValenciaDivClean::Tags::TildeD, tmpl::size_t<3>,           \
                   Frame::Inertial>,                                          \
      ::Tags::Flux<ValenciaDivClean::Tags::TildeTau, tmpl::size_t<3>,         \
                   Frame::Inertial>,                                          \
      ::Tags::Flux<ValenciaDivClean::Tags::TildeS<Frame::Inertial>,           \
                   tmpl::size_t<3>, Frame::Inertial>,                         \
      ::Tags::Flux<ValenciaDivClean::Tags::TildeB<Frame::Inertial>,           \
                   tmpl::size_t<3>, Frame::Inertial>,                         \
      ::Tags::Flux<ValenciaDivClean::Tags::TildePhi, tmpl::size_t<3>,         \
                   Frame::Inertial>,                                          \
      gr::Tags::Lapse<>, gr::Tags::Shift<3, Frame::Inertial, DataVector>,     \
      gr::Tags::SpatialMetric<3>, gr::Tags::SqrtDetSpatialMetric<DataVector>, \
      gr::Tags::InverseSpatialMetric<3, Frame::Inertial, DataVector>,         \
      evolution::dg::Actions::detail::NormalVector<3>>

#define INSTANTIATION(r, data)                                                 \
  template void MonotisedCentralPrim::reconstruct(                             \
      gsl::not_null<std::array<Variables<TAGS_LIST(data)>, 3>*>                \
          vars_on_lower_face,                                                  \
      gsl::not_null<std::array<Variables<TAGS_LIST(data)>, 3>*>                \
          vars_on_upper_face,                                                  \
      const Variables<hydro::grmhd_tags<DataVector>>& volume_prims,            \
      const Variables<typename System::variables_tag::type::tags_list>&        \
          volume_spacetime_and_cons_vars,                                      \
      const EquationsOfState::EquationOfState<true, THERMO_DIM(data)>& eos,    \
      const Element<3>& element,                                               \
      const FixedHashMap<                                                      \
          maximum_number_of_neighbors(3) + 1,                                  \
          std::pair<Direction<3>, ElementId<3>>, std::vector<double>,          \
          boost::hash<std::pair<Direction<3>, ElementId<3>>>>& neighbor_data,  \
      const Mesh<3>& subcell_mesh) const;                                      \
  template void MonotisedCentralPrim::reconstruct_fd_neighbor(                 \
      gsl::not_null<Variables<TAGS_LIST(data)>*> vars_on_face,                 \
      const Variables<hydro::grmhd_tags<DataVector>>& subcell_volume_prims,    \
      const Variables<tmpl::list<                                              \
          gr::Tags::SpacetimeMetric<3>, GeneralizedHarmonic::Tags::Phi<3>,     \
          GeneralizedHarmonic::Tags::Pi<3>>>& subcell_volume_spacetime_metric, \
      const EquationsOfState::EquationOfState<true, THERMO_DIM(data)>& eos,    \
      const Element<3>& element,                                               \
      const FixedHashMap<                                                      \
          maximum_number_of_neighbors(3) + 1,                                  \
          std::pair<Direction<3>, ElementId<3>>, std::vector<double>,          \
          boost::hash<std::pair<Direction<3>, ElementId<3>>>>& neighbor_data,  \
      const Mesh<3>& subcell_mesh,                                             \
      const Direction<3> direction_to_reconstruct) const;

GENERATE_INSTANTIATIONS(INSTANTIATION, (1, 2))

#undef INSTANTIATION
#undef TAGS_LIST
#undef THERMO_DIM
}  // namespace grmhd::GhValenciaDivClean::fd