// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY 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 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY 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 FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:50:19

#ifndef MSSMNoFVatMGUT_PHYSICAL_H
#define MSSMNoFVatMGUT_PHYSICAL_H

#include "linalg2.hpp"
#include <Eigen/Core>

#include <iosfwd>
#include <string>

namespace flexiblesusy {

struct MSSMNoFVatMGUT_physical {
   MSSMNoFVatMGUT_physical();
   void clear();
   void convert_to_hk();   ///< converts pole masses to HK convention
   void convert_to_slha(); ///< converts pole masses to SLHA convention
   Eigen::ArrayXd get() const; ///< returns array with all masses and mixings
   void set(const Eigen::ArrayXd&); ///< set all masses and mixings
   Eigen::ArrayXd get_masses() const; ///< returns array with all masses
   void set_masses(const Eigen::ArrayXd&); ///< set all masses
   void print(std::ostream&) const;

   double MVG;
   double MGlu;
   double MFd;
   double MFs;
   double MFb;
   double MFu;
   double MFc;
   double MFt;
   double MFve;
   double MFvm;
   double MFvt;
   double MFe;
   double MFm;
   double MFtau;
   double MSveL;
   double MSvmL;
   double MSvtL;
   Eigen::Array<double,2,1> MSd;
   Eigen::Array<double,2,1> MSu;
   Eigen::Array<double,2,1> MSe;
   Eigen::Array<double,2,1> MSm;
   Eigen::Array<double,2,1> MStau;
   Eigen::Array<double,2,1> MSs;
   Eigen::Array<double,2,1> MSc;
   Eigen::Array<double,2,1> MSb;
   Eigen::Array<double,2,1> MSt;
   Eigen::Array<double,2,1> Mhh;
   Eigen::Array<double,2,1> MAh;
   Eigen::Array<double,2,1> MHpm;
   Eigen::Array<double,4,1> MChi;
   Eigen::Array<double,2,1> MCha;
   double MVWm;
   double MVP;
   double MVZ;

   Eigen::Matrix<double,2,2> ZD;
   Eigen::Matrix<double,2,2> ZU;
   Eigen::Matrix<double,2,2> ZE;
   Eigen::Matrix<double,2,2> ZM;
   Eigen::Matrix<double,2,2> ZTau;
   Eigen::Matrix<double,2,2> ZS;
   Eigen::Matrix<double,2,2> ZC;
   Eigen::Matrix<double,2,2> ZB;
   Eigen::Matrix<double,2,2> ZT;
   Eigen::Matrix<double,2,2> ZH;
   Eigen::Matrix<double,2,2> ZA;
   Eigen::Matrix<double,2,2> ZP;
   Eigen::Matrix<std::complex<double>,4,4> ZN;
   Eigen::Matrix<std::complex<double>,2,2> UM;
   Eigen::Matrix<std::complex<double>,2,2> UP;
   Eigen::Matrix<double,2,2> ZZ;

};

std::ostream& operator<<(std::ostream&, const MSSMNoFVatMGUT_physical&);

} // namespace flexiblesusy

#endif


/*
Copyright 2016-2017 Robotics and Biology Lab, TU Berlin. All rights reserved.

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.

THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those of the authors and should not be interpreted as representing official policies, either expressed or implied, of the FreeBSD Project.
*/

#include <ecto_pcl/ecto_pcl.hpp>
#include <ecto_pcl/pcl_cell.hpp>
#include <ecto_pcl/pcl_cell_with_normals.hpp>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/common/io.h>
#include <pcl/common/angles.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/common/eigen.h>

#include <pcl_conversions/pcl_conversions.h>

#include <ros/ros.h>
#include <ros/console.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <pregrasp_msgs/GraspStrategyArray.h>
#include <ecto_rbo_grasping/PoseSet.h>

#include <Eigen/Geometry>

using namespace ecto;

namespace ecto_rbo_grasping
{

typedef Eigen::Matrix<float, 4, 1, Eigen::DontAlign> UnalignedVector4f;
typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> UnalignedVector3f;
typedef Eigen::Matrix<float, 2, 1, Eigen::DontAlign> UnalignedVector2f;
typedef Eigen::Transform<float,3,Eigen::Affine,Eigen::DontAlign> UnalignedAffine3f;

struct PushingMotions
{
    ecto::spore<pregrasp_msgs::GraspStrategyArrayConstPtr> pushing_pregrasp_messages_;
    ecto::spore< ::posesets::PoseSetArrayConstPtr> pushing_manifolds_;

    spore<std::vector< ::pcl::PointIndices > > clusters_;
    spore<std::vector< ::pcl::PointIndices > > cluster_seeds_;
    spore<std::vector< ::pcl::PointIndices > > cluster_borders_;

    ros::Time last_marker_message_;

    // parameters
    //  spore<double> distance_threshold_;
    //  spore<double> min_inlier_ratio_;
    //  spore<double> weight_contour_;
    //  spore<double> min_boxness_;
    //  spore<double> max_size_;
    //  spore<double> min_size_;

    static void declare_params(ecto::tendrils& params)
    {
        //    params.declare<double>("min_inlier_ratio", "Minimum number of inlier points per plane.", 0.5);
        //    params.declare<double>("distance_threshold", "Maximum mean error a plane fit may have.", 0.05);
        //    params.declare<double>("min_boxness", "", 0.8);
        //    params.declare<double>("weight_contour", "", 0.5);
        //    params.declare<double>("max_size", "", 0.24);
        //    params.declare<double>("min_size", "", 0.08);
    }

    static void declare_io(const tendrils& params, tendrils& inputs, tendrils& outputs)
    {
        inputs.declare<std::vector< ::pcl::PointIndices> >("clusters", "Clusters.");
        inputs.declare<std::vector< ::pcl::PointIndices> >("cluster_seeds", "Clusters.");
        inputs.declare<std::vector< ::pcl::PointIndices> >("cluster_borders", "Clusters.");

        outputs.declare<pregrasp_msgs::GraspStrategyArrayConstPtr>("pushing_pregrasp_messages", "All the grasps that should be used.");
        outputs.declare< ::posesets::PoseSetArrayConstPtr>("pushing_manifolds", "All the grasps that should be used.");
    }

    void configure(const tendrils& params, const tendrils& inputs, const tendrils& outputs)
    {
        clusters_ = inputs["clusters"];
        cluster_seeds_ = inputs["cluster_seeds"];
        cluster_borders_ = inputs["cluster_borders"];

        pushing_pregrasp_messages_ = outputs["pushing_pregrasp_messages"];
        pushing_manifolds_ = outputs["pushing_manifolds"];

        last_marker_message_ = ros::Time::now();
    }

    inline void computeNormalMean(const ::pcl::PointCloud< ::pcl::Normal>& normals, const ::pcl::PointIndices& indices, Eigen::Vector3f& mean_normal)
    {
        mean_normal.setZero();
        for (size_t i = 0; i < indices.indices.size(); ++i)
            mean_normal += normals.at(indices.indices[i]).getNormalVector3fMap();

        mean_normal.normalize();
    }

    template<typename Point>
    inline float pointToLineDistance(const Point p, ::pcl::ModelCoefficientsConstPtr& line)
    {
        //    ::Eigen::Vector4f point(p.x, p.y, p.z, 0.0f);
        //    ::Eigen::Vector4f line_point(line->values[0], line->values[1], line->values[2], 0.0f);
        //    ::Eigen::Vector4f line_direction(line->values[3], line->values[4], line->values[5], 0.0f);
        //    return ::pcl::sqrPointToLineDistance(point, line_point, line_direction);
        return (p.x-line->values[0]) * (p.x-line->values[0]) + (p.y-line->values[1]) * (p.y-line->values[1]) + (p.z-line->values[2]) * (p.z-line->values[2]);
    }

    template<typename Point>
    int process(const tendrils& inputs, const tendrils& outputs,
                boost::shared_ptr<const ::pcl::PointCloud<Point> >& input,
                boost::shared_ptr<const ::pcl::PointCloud< ::pcl::Normal> >& normals)
    {
        pregrasp_msgs::GraspStrategyArrayPtr push_messages(new ::pregrasp_msgs::GraspStrategyArray());
        ::posesets::PoseSetArrayPtr pushing_manifolds(new ::posesets::PoseSetArray());

        push_messages->header = pcl_conversions::fromPCL(input->header);

        for (std::vector< ::pcl::PointIndices >::iterator it = clusters_->begin(); it != clusters_->end(); ++it)
        {
            size_t index = std::distance(clusters_->begin(), it);

            if (clusters_->at(index).indices.empty() || cluster_seeds_->at(index).indices.empty() || cluster_borders_->at(index).indices.empty())
                continue;

//            ROS_INFO("Generating Push No. %zu.: %zu (cluster size), %zu (seed size), %zu (border size)", index, clusters_->at(index).indices.size(), cluster_seeds_->at(index).indices.size(), cluster_borders_->at(index).indices.size());

            ::pregrasp_msgs::GraspStrategy msg;
            msg.pregrasp_configuration = pregrasp_msgs::GraspStrategy::PREGRASP_HOOK;
            msg.strategy = pregrasp_msgs::GraspStrategy::STRATEGY_PUSH;

            // start of slide
            Eigen::Vector4f seed_centroid, border_centroid;
            Eigen::Vector3f mean_normal(0, 0, 0);
            ::pcl::compute3DCentroid(*input, cluster_seeds_->at(index), seed_centroid);
            ::pcl::compute3DCentroid(*input, cluster_borders_->at(index), border_centroid);
            computeNormalMean(*normals, cluster_seeds_->at(index), mean_normal);
            msg.pregrasp_pose.center.pose.position.x = seed_centroid(0);
            msg.pregrasp_pose.center.pose.position.y = seed_centroid(1);
            msg.pregrasp_pose.center.pose.position.z = seed_centroid(2);
            /*
            tf::Vector3 z_axis(0, 0, -1);
            tf::Vector3 avg_normal(mean_normal(0), mean_normal(1), mean_normal(2));
            tf::quaternionTFToMsg(tf::shortestArcQuat(z_axis, avg_normal), msg.pregrasp_pose.center.pose.orientation);
            */
            Eigen::Vector3f push_direction = (border_centroid.topRows(3) - seed_centroid.topRows(3)).normalized();
            Eigen::Vector3f approach_z = -mean_normal;
            Eigen::Vector3f approach_x = push_direction.cross(approach_z);
            Eigen::Matrix3f rotation;
            rotation << approach_x, approach_z.cross(approach_x), approach_z;
            Eigen::Quaterniond q_eigen(rotation.cast<double>());
            tf::Quaternion q_tf;
            tf::quaternionEigenToTF(q_eigen, q_tf);
            tf::quaternionTFToMsg(q_tf, msg.pregrasp_pose.center.pose.orientation);

            ::pcl::PointCloud<Point> tmp_cloud;
            ::Eigen::Affine3f seed_transform(::Eigen::Translation3f(seed_centroid.topRows(3)));
            seed_transform.rotate(::Eigen::AngleAxisf(q_eigen.cast<float>()));
            ::pcl::transformPointCloud(*input, cluster_seeds_->at(index), tmp_cloud, seed_transform.inverse());
            ::Eigen::Vector4f max_point, min_point;
            ::pcl::getMinMax3D(tmp_cloud, min_point, max_point);
            ::Eigen::Vector4f difference = max_point - min_point;

            msg.pregrasp_pose.size.resize(4);
            msg.pregrasp_pose.size[0] = difference[0];
            msg.pregrasp_pose.size[1] = difference[1];
            msg.pregrasp_pose.size[2] = difference[2];
            msg.pregrasp_pose.size[3] = 0.1;

            // end of slide
            // HINT: could also be the mean of the seeds region
            computeNormalMean(*normals, cluster_borders_->at(index), mean_normal);
            msg.pregrasp_pose.pose.pose.position.x = border_centroid(0);
            msg.pregrasp_pose.pose.pose.position.y = border_centroid(1);
            msg.pregrasp_pose.pose.pose.position.z = border_centroid(2);

            /*
            avg_normal.setValue(mean_normal(0), mean_normal(1), mean_normal(2));
            tf::quaternionTFToMsg(tf::shortestArcQuat(z_axis, avg_normal), msg.pregrasp_pose.pose.pose.orientation);
            */
            approach_z = -mean_normal;
            approach_x = push_direction.cross(approach_z);
            rotation << approach_x, approach_z.cross(approach_x), approach_z;
            q_eigen = rotation.cast<double>();
            tf::quaternionEigenToTF(q_eigen, q_tf);
            tf::quaternionTFToMsg(q_tf, msg.pregrasp_pose.pose.pose.orientation);

            tmp_cloud.clear();
            ::Eigen::Affine3f border_transform(::Eigen::Translation3f(border_centroid.topRows(3)));
            border_transform.rotate(::Eigen::AngleAxisf(q_eigen.cast<float>()));
            ::pcl::transformPointCloud(*input, cluster_borders_->at(index), tmp_cloud, border_transform.inverse());
            ::pcl::getMinMax3D(tmp_cloud, min_point, max_point);
            difference = max_point - min_point;

//            sensor_msgs::PointCloud2 color_msg;
//            ::pcl::toROSMsg(tmp_cloud, color_msg);
//            color_msg.header.frame_id = input->header.frame_id;
//            color_msg.header.stamp = ::ros::Time::now();
//            static ::ros::NodeHandle nh;
//            static ::ros::Publisher debug_publisher = nh.advertise< sensor_msgs::PointCloud2 > ("/debug_shit", 1);
//            debug_publisher.publish(color_msg);

            msg.pregrasp_pose.image_size.resize(4);
            msg.pregrasp_pose.image_size[0] = difference[0];
            msg.pregrasp_pose.image_size[1] = difference[1];
            msg.pregrasp_pose.image_size[2] = difference[2];
            msg.pregrasp_pose.image_size[3] = 0.1;

            // set object pose relative to hand
            msg.object.center.pose = msg.object.pose.pose = msg.pregrasp_pose.center.pose;
            msg.object.size.push_back(0.1);
            msg.object.size.push_back(0.1);
            msg.object.size.push_back(0.07);
            msg.object.size.push_back(4.0);
            msg.object.image_size.push_back(0.01);
            msg.object.image_size.push_back(0.1);
            msg.object.image_size.push_back(0.1);
            msg.object.image_size.push_back(4.0);

            push_messages->strategies.push_back(msg);

            // add corresponding manifold
            ::posesets::PoseSet ps(tf::Transform(q_tf, tf::Vector3(seed_centroid(0), seed_centroid(1), seed_centroid(2))));
            ps.setPositions(tf::Vector3(difference[0], difference[1], difference[2]));
            ps.getOrientations().add(q_tf, tf::Vector3(approach_z(0), approach_z(1), approach_z(2)));
            pushing_manifolds->push_back(ps);

            ROS_INFO("Push pointing from: %f, %f, %f", msg.pregrasp_pose.center.pose.position.x, msg.pregrasp_pose.center.pose.position.y, msg.pregrasp_pose.center.pose.position.z);
            ROS_INFO("                to: %f, %f, %f", msg.pregrasp_pose.pose.pose.position.x, msg.pregrasp_pose.pose.pose.position.y, msg.pregrasp_pose.pose.pose.position.z);
        }

        (*pushing_pregrasp_messages_) = push_messages;
        (*pushing_manifolds_) = pushing_manifolds;

        return OK;
    }
};

}

ECTO_CELL(ecto_rbo_grasping, ecto::pcl::PclCellWithNormals<ecto_rbo_grasping::PushingMotions>, "PushingMotions", "Finding pushing motions.");


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>
#include <CompilerConfig.h>

using namespace PartDesignGui;

PCETransform::PCETransform(
	Vector3d2 surfs_,
	Vector3d1 surf_normals_,
	Vector3d center_,
	Vector3d rotation_,
	glm::dmat4 PM_,
	glm::dmat4 RM_,
	Vector3d tranform_corner_) :
	surfs(surfs_),
	surf_normals(surf_normals_),
	center(center_),
	rotation(rotation_),
	PM(PM_),
	RM(RM_),
	tranform_corner(tranform_corner_) {

};

PCETransform::PCETransform()
{
	index = -1;
}

bool PCETransform::CheckWL(const int& wl_index_) const
{
	for (auto wl_rate : wl_rates)
	{
		if (wl_rate.wl->index == wl_index_)
			return true;
	}
	return false;
}

void PCETransform::Clear()
{
	std::vector<WLRATE>().swap(wl_rates);
	cut_ends.Clear();
	Vector3d2().swap(surfs);
	Vector3d1().swap(surf_normals);
}

void PCETransform::AddNewEdges(std::vector<PCEEdge>& edges_, std::vector<int>& edge_indexes)
{
	edge_indexes.clear();
	if (cut_ends.valid)
	{
		for (const auto& end : cut_ends.ends)
		{
			edges_.emplace_back(PCEEdge(edges_.size(), pce_shape->index, end.cut));
			edge_indexes.emplace_back(edges_.back().index);
		}
	}
	else
		AddBoundingEdges(edges_, edge_indexes);
}

void PCETransform::AddBoundingEdges(std::vector<PCEEdge>& edges_, std::vector<int>& edge_indexes)
{
	edge_indexes.clear();

	auto& offset_points = bounding_box.offset_2d;
	auto& origin_points = bounding_box.origin_2d;

	double height = wl_rates[0].wl->size[2];
	for (int i = 0; i < offset_points.size(); i++)
	{
		const int index_0 = i;
		const int index_1 = (i + 1) % offset_points.size();

		const Vector2d origin_s = origin_points[index_0];
		const Vector2d origin_e = origin_points[index_1];
		const Vector2d offset_s = offset_points[index_0];
		const Vector2d offset_e = offset_points[index_1];

		PCECUT cut;
		cut.origin_s = origin_s;
		cut.origin_e = origin_e;
		cut.offset_s = offset_s;
		cut.offset_e = offset_e;
		cut.cutting_surface_normal = bounding_box.normals[i];
		cut.u_cutting_line_s = Vector3d(offset_s[0], offset_s[1], height);
		cut.u_cutting_line_e = Vector3d(offset_e[0], offset_e[1], height);
		cut.l_cutting_line_s = Vector3d(offset_s[0], offset_s[1], 0.0);
		cut.l_cutting_line_e = Vector3d(offset_e[0], offset_e[1], 0.0);

		cut.l_u_vector = Vector3d(0.0, 0.0, height);

		edges_.emplace_back(PCEEdge(edges_.size(), pce_shape->index, cut));
		edge_indexes.emplace_back(edges_.back().index);
	}
}

//only for collision detection
Vector2d1 PCETransform::UOffset(const Vector2d t) const
{
	auto points = cut_ends.valid ? cut_ends.u_offset_2d : bounding_box.offset_2d;
	for (auto& p : points)p += t;
	return points;
}

//only for collision detection
Vector2d1 PCETransform::LOffset(const Vector2d t) const
{
	auto points = cut_ends.valid ? cut_ends.l_offset_2d : bounding_box.offset_2d;
	for (auto& p : points)p += t;
	return points;
}

//only for collision detection
Vector2d1 PCETransform::UOrigin(const Vector2d t) const
{
	auto points = cut_ends.valid ? cut_ends.u_origin_2d : bounding_box.origin_2d;
	for (auto& p : points) p += t;
	return points;
}

//only for collision detection
Vector2d1 PCETransform::LOrigin(const Vector2d t) const
{
	auto points = cut_ends.valid ? cut_ends.l_origin_2d : bounding_box.origin_2d;
	for (auto& p : points) p += t;
	return points;
}

Vector2d1 PCETransform::ULOffset(const Vector2d t) const
{
	auto points = cut_ends.valid ? cut_ends.ul_offset_2d : bounding_box.offset_2d;
	for (auto& p : points)p += t;
	return points;
}

Vector2d1 PCETransform::ULOrigin(const Vector2d t) const
{
	auto points = cut_ends.valid ? cut_ends.ul_origin_2d : bounding_box.origin_2d;
	for (auto& p : points)p += t;
	return points;
}

PCETransform::CutEnds PCETransform::BuildEnd(const double& kerf, const HMODULE& hModule,
	const Vector3d2& surfs_, Vector3d upper_normal, Vector3d lower_normal)
{
	//build data structure
	struct Node
	{
	public:
		enum { EdgeNode, SurfNode };
		enum { UpperEdge, LowerEdge };

		int index, style;
		std::vector<int> neighbors;
		std::vector<int> ends;

		int surface_index;

		bool used;

		//edge
		Vector3d edge_s, edge_e;
		int edge_style;
		int friend_node;
		int edge_index;

		//surface
		Vector3d1 surf_points;
		Vector3d surf_normal;

		//EdgeNode
		Node(int index_, Vector3d s_, Vector3d e_, int edge_style_, int edge_index_, int surface_index_) :
			index(index_), style(EdgeNode), edge_s(s_), edge_e(e_), edge_style(edge_style_), edge_index(edge_index_), surface_index(surface_index_)
		{
			friend_node = -1;
			used = false;
		};

		//SurfNode
		Node(int index_, const Vector3d1& points_, const Vector3d& surf_normal_, int surface_index_) :
			index(index_), style(SurfNode), surf_points(points_), surf_normal(surf_normal_), surface_index(surface_index_)
		{
			edge_style = -1;
			friend_node = -1;
			used = false;
		};

		void PushEnd(int end)
		{
			if (Math::Functs::VectorIndex(ends, end) == -1) ends.emplace_back(end);
		}

		void PushEnd(std::vector<int> ints)
		{
			for (auto i : ints) PushEnd(i);
		}
	};
	struct Edge
	{
	public:
		int index;
		int node_0, node_1;

		Edge(int index_, int n_0, int n_1) : index(index_), node_0(n_0), node_1(n_1)
		{
		};
	};
	auto CheckConnecting0 = [&](const Vector3d& s, const Vector3d& e, const Vector3d1& contour)
	{
		for (int i = 0; i < contour.size(); i++)
		{
			Vector3d s_ = contour[i];
			Vector3d e_ = contour[(i + 1) % contour.size()];
			bool ss = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(s, s_));
			bool se = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(s, e_));
			bool es = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(e, s_));
			bool ee = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(e, e_));
			if ((ss && ee) || (se && es)) return true;
		}
		return false;
	};
	auto CheckConnecting1 = [&](Vector3d1& contour0, Vector3d1& contour1)
	{
		for (int i = 0; i < contour0.size(); i++)
		{
			Vector3d s = contour0[i];
			Vector3d e = contour0[(i + 1) % contour0.size()];

			for (int j = 0; j < contour1.size(); j++)
			{
				Vector3d s_ = contour1[j];
				Vector3d e_ = contour1[(j + 1) % contour1.size()];
				bool ss = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(s, s_));
				bool se = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(s, e_));
				bool es = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(e, s_));
				bool ee = Math::Functs::IsAlmostZero(Math::Functs::GetDistance(e, e_));
				if ((ss && ee) || (se && es)) return true;
			}
		}
		return false;
	};

	//searching for the upper surface and the lower surface
	auto BuildNodes = [&](std::vector<Node>& nodes, Vector3d1& surf_normals_, PCETransform::CutEnds& cut_ends_) {

		bool ub(false), lb(false);
		for (int i = 0; i < surf_normals_.size(); i++)
		{
			if (Math::Functs::IsAlmostZero(Math::Functs::GetAngleBetween(upper_normal, surf_normals_[i])))
			{
				if (ub)
				{
					std::cerr << "Find not single surfaces which are upper surfaces..." << std::endl;
					system("pause");
					return false;
				}

				cut_ends_.u_surf = surfs_[i];
				cut_ends_.u_surf_index = i;

				ub = true;
				for (int j = 0; j < cut_ends_.u_surf.size(); j++)
					nodes.emplace_back(Node(nodes.size(), cut_ends_.u_surf[j], cut_ends_.u_surf[(j + 1) % cut_ends_.u_surf.size()], Node::UpperEdge, j, i));
			}
			else
			{
				if (Math::Functs::IsAlmostZero(Math::Functs::GetAngleBetween(lower_normal, surf_normals_[i])))
				{
					if (lb)
					{
						std::cerr << "Find not single surfaces which are lower surfaces..." << std::endl;
						system("pause");
						return false;
					}

					cut_ends_.l_surf = surfs_[i];
					cut_ends_.l_surf_index = i;

					lb = true;
					for (int j = 0; j < cut_ends_.l_surf.size(); j++)
						nodes.emplace_back(Node(nodes.size(), cut_ends_.l_surf[j], cut_ends_.l_surf[(j + 1) % cut_ends_.l_surf.size()], Node::LowerEdge, j, i));
				}
				else
				{
					nodes.emplace_back(Node(nodes.size(), surfs_[i], surf_normals_[i], i));
				}
			}
		}
		if (!ub || !lb) return false;
		return true;
	};

	//build edges
	auto BuildEdges = [&](std::vector<Node>& nodes)
	{
		std::vector<Edge> edges;
		for (auto& n : nodes)
		{
			for (auto& nn : nodes)
			{
				if (n.index < nn.index && n.style == Node::EdgeNode && nn.style == Node::SurfNode)
				{
					if (CheckConnecting0(n.edge_s, n.edge_e, nn.surf_points))
					{
						edges.emplace_back(edges.size(), n.index, nn.index);
						n.neighbors.emplace_back(nn.index);
						nn.neighbors.emplace_back(n.index);
					}
				}

				if (n.index < nn.index && n.style == Node::SurfNode && nn.style == Node::EdgeNode)
				{
					if (CheckConnecting0(nn.edge_s, nn.edge_e, n.surf_points))
					{
						edges.emplace_back(edges.size(), n.index, nn.index);
						n.neighbors.emplace_back(nn.index);
						nn.neighbors.emplace_back(n.index);
					}
				}

				if (n.index < nn.index && n.style == Node::SurfNode && nn.style == Node::SurfNode)
				{
					if (CheckConnecting1(n.surf_points, nn.surf_points))
					{
						edges.emplace_back(edges.size(), n.index, nn.index);
						n.neighbors.emplace_back(nn.index);
						nn.neighbors.emplace_back(n.index);
					}
				}
			}
		}
		return edges;
	};

	//Note: there are challenged case I can not handle
	//This algorithm cannot guarantee to decompose all of surface into valid ends
	//multi merge
	auto RegionGrow = [&](std::vector<Node>& nodes, PCETransform::CutEnds& cut_ends_)
	{
		for (auto& n : nodes)
		{
			if (n.style == Node::EdgeNode)
			{
				nodes[n.neighbors[0]].PushEnd(n.index);
				n.used = true;
			}
		}

		while (true)
		{
			for (auto& n : nodes)
			{
				if (n.style == Node::SurfNode)
				{
					if (n.ends.size() > 2)
					{
						std::cerr << "Nodes' ends > 2..." << std::endl;
						system("pause");
						return false;
					}
					if (n.ends.size() == 2)
					{
						Node& n0 = nodes[n.ends[0]];
						Node& n1 = nodes[n.ends[1]];
						if (n0.friend_node < 0)n0.friend_node = n1.index;
						if (n1.friend_node < 0)n1.friend_node = n0.index;
						n0.PushEnd(n.index);
						n1.PushEnd(n.index);
						n.used = true;
					}
					if (n.ends.size() == 1)
					{
						Node& n_ = nodes[n.ends[0]];
						n_.PushEnd(n.index);
					}
				}
			}

			bool goon = false;
			for (auto& n : nodes)
			{
				if (n.style == Node::SurfNode && n.ends.size() == 0)
				{
					goon = true;
					break;
				}
				if (n.style == Node::EdgeNode && n.friend_node < 0)
				{
					goon = true;
					break;
				}
			}

			if (!goon) break;

			for (auto& n : nodes)
			{
				if (n.style == Node::SurfNode && !n.used && n.ends.size() > 0)
				{
					for (auto i : n.neighbors)
					{
						if (!nodes[i].used && nodes[i].ends.size() < 2)
							nodes[i].PushEnd(n.ends);
					}
					n.used = true;
				}
			}
		}

		//valid detection
		for (auto& n : nodes)
		{
			if (n.style == Node::EdgeNode && n.friend_node == -1)
			{
				std::cerr << "End computing error...: n.friend_node==-1" << std::endl;
				system("pause");
				return false;
			}
			if (n.style == Node::SurfNode && n.ends.size() == 0)
			{
				std::cerr << "End computing error...:  n.ends.size()==0" << std::endl;
				system("pause");
				return false;
			}
		}

		//build ends
		for (auto& n : nodes)
		{
			if (n.style == Node::EdgeNode && n.edge_style == Node::UpperEdge)
			{
				Vector3d2 con_surfs;
				std::vector<int> con_surf_indexes;
				Vector3d1 con_surf_normals;
				n.PushEnd(nodes[n.friend_node].ends);
				for (auto i : n.ends)
				{
					con_surfs.emplace_back(nodes[i].surf_points);
					con_surf_normals.emplace_back(nodes[i].surf_normal);
					con_surf_indexes.emplace_back(nodes[i].surface_index);
				}

				if (con_surfs.size() != 1)
				{
					std::cerr << "Two ends cutting...." << std::endl;
					return false;
				}
				cut_ends_.ends.emplace_back(PCEEnd(kerf, n.edge_index, n.edge_s, n.edge_e,
					nodes[n.friend_node].edge_s, nodes[n.friend_node].edge_e, con_surfs, con_surf_normals, con_surf_indexes));
			}
		}
	};

	//Note: this function can only handle very simple case
	auto ConnectContours = [](const HMODULE& hModule_, const std::vector<std::pair<Vector2d, Vector2d>>& contours)
	{
		Vector2d1 results;
		auto line_itersection = (CGAL_2D_Intersection_Line_Line)GetProcAddress(hModule_, "CGAL_2D_Intersection_Line_Line");
		for (int i = 0; i < contours.size(); i++)
		{
			int index_0 = i;
			int index_1 = (i + 1) % contours.size();
			Vector2d inter;
			line_itersection(contours[index_0].first, contours[index_0].second, contours[index_1].first, contours[index_1].second, inter);
			results.emplace_back(inter);
		}
		return results;
	};

	PCETransform::CutEnds cut_ends_;

	cut_ends_.valid = false;
	Vector3d1 surf_normals_;
	for (const auto& surf_ : surfs_)
	{
		auto n = Math::Functs::ComputeNormalFromPolyline(surf_);
		Math::Functs::SetVectorLength(n, 1.0);
		surf_normals_.emplace_back(-n[0], -n[1], -n[2]);
	}

	//input valid detection
	for (auto& s : surfs_)
		if (s.size() < 3)
		{
			std::cerr << "Input surface is not valid..." << std::endl;
			system("pause");
			return cut_ends_;
		}

	if (surfs_.size() != surf_normals_.size())
	{
		std::cerr << "Input surface and normals are not valid..." << std::endl;
		system("pause");
		return cut_ends_;
	}

	//build nodes and edges
	std::vector<Node> nodes;
	if (!BuildNodes(nodes, surf_normals_, cut_ends_)) return cut_ends_;
	std::vector<Edge> edges = BuildEdges(nodes);

	//valid detection
	for (auto& n : nodes)
	{
		if (n.style == Node::EdgeNode && n.neighbors.size() != 1)
		{
			std::cerr << "Edge computing error..." << std::endl;
			system("pause");
			return cut_ends_;
		}
		if (n.style == Node::SurfNode && n.neighbors.size() != n.surf_points.size())
		{
			std::cerr << "Edge computing error..." << std::endl;
			system("pause");
			return cut_ends_;
		}
	}

	//Note: there are challenged case I can not handle
	//This algorithm cannot guarantee to decompose all of surface into valid ends
	//multi merge
	RegionGrow(nodes, cut_ends_);

	//sort ends
	auto SortEnd = [](PCEEnd& end0, PCEEnd& end1) { return  end0.index < end1.index; };
	std::sort(cut_ends_.ends.begin(), cut_ends_.ends.end(), SortEnd);

	//kerf offset
	auto check_orientation = (CGAL_2D_Polygon_Is_Clockwise_Oriented)GetProcAddress(hModule, "CGAL_2D_Polygon_Is_Clockwise_Oriented");
	auto PolyOffset = (CGAL_2D_Polygon_One_Offsets)GetProcAddress(hModule, "CGAL_2D_Polygon_One_Offsets");
	auto PolyUnion = (CGAL_2D_Two_Polygons_Union)GetProcAddress(hModule, "CGAL_2D_Two_Polygons_Union");

	//u_origin_2d  l_origin_2d
	for (auto p : cut_ends_.u_surf) cut_ends_.u_origin_2d.emplace_back(p.x, p.y);
	for (auto p : cut_ends_.l_surf) cut_ends_.l_origin_2d.emplace_back(p.x, p.y);

	//ul_origin_2d
	Vector2d2 polygons;
	polygons.clear();
	PolyUnion(cut_ends_.u_origin_2d, cut_ends_.l_origin_2d, polygons);
	cut_ends_.ul_origin_2d = polygons[0];

	//u_offset_2d
	std::vector<std::pair<Vector2d, Vector2d>> contours;
	for (auto& end : cut_ends_.ends)
	{
		contours.emplace_back(Vector2d(end.cut.u_cutting_line_s[0], end.cut.u_cutting_line_s[1]),
			Vector2d(end.cut.u_cutting_line_e[0], end.cut.u_cutting_line_e[1]));
	}
	cut_ends_.u_offset_2d = ConnectContours(hModule, contours);

	//l_offset_2d
	contours.clear();
	for (auto& end : cut_ends_.ends)
	{
		contours.emplace_back(Vector2d(end.cut.l_cutting_line_s[0], end.cut.l_cutting_line_s[1]),
			Vector2d(end.cut.l_cutting_line_e[0], end.cut.l_cutting_line_e[1]));
	}
	cut_ends_.l_offset_2d = ConnectContours(hModule, contours);

	//valid detection
	if (cut_ends_.u_offset_2d.size() != cut_ends_.l_offset_2d.size())
	{
		std::cerr << "if (u_offset_2d.size() != l_offset_2d.size())" << std::endl;
		system("pause");
		return cut_ends_;
	}

	//ul_offset_2d
	polygons.clear();
	PolyUnion(cut_ends_.u_offset_2d, cut_ends_.l_offset_2d, polygons);
	cut_ends_.ul_offset_2d = polygons[0];

	cut_ends_.ul_origin_2d=Math::Functs::Polygon_Clear(cut_ends_.ul_origin_2d, CompilerConfig::Instance().GetAngleMatchError(), CompilerConfig::Instance().GetPartMatchError());
	cut_ends_.ul_offset_2d=Math::Functs::Polygon_Clear(cut_ends_.ul_offset_2d, CompilerConfig::Instance().GetAngleMatchError(), CompilerConfig::Instance().GetPartMatchError());

	cut_ends_.valid = true;
	return cut_ends_;
}

bool PCETransform::BuildEnd(const double& kerf, const HMODULE& hModule, Vector3d upper_normal, Vector3d lower_normal)
{
	cut_ends = BuildEnd(kerf, hModule, surfs, upper_normal, lower_normal);
	return cut_ends.valid;
}

bool PCETransform::BuildBoundingBox(const double& kerf, const HMODULE& hModule)
{
	auto GetBoundingBox_2D = [](const Vector2d2& points, double kerf_ = 0.0)
	{
		Vector2d corner_0, corner_2;
		Math::Functs::GetBoundingBox(points, corner_0, corner_2);
		Vector2d1 box;
		box.emplace_back(Vector2d(corner_0[0] - kerf_ / 2.0, corner_0[1] - kerf_ / 2.0));
		box.emplace_back(Vector2d(corner_2[0] + kerf_ / 2.0, corner_0[1] - kerf_ / 2.0));
		box.emplace_back(Vector2d(corner_2[0] + kerf_ / 2.0, corner_2[1] + kerf_ / 2.0));
		box.emplace_back(Vector2d(corner_0[0] - kerf_ / 2.0, corner_2[1] + kerf_ / 2.0));
		return box;
	};

	Vector2d2 surfs_2d = Math::Functs::Vector3d2d(surfs);

	bounding_box.origin_2d = GetBoundingBox_2D(surfs_2d);
	bounding_box.offset_2d = GetBoundingBox_2D(surfs_2d, kerf);
	bounding_box.center_2d = (bounding_box.origin_2d[0] + bounding_box.origin_2d[2]) / 2.0;

	bounding_box.normals.emplace_back(Vector3d(0.0, -1.0, 0.0));
	bounding_box.normals.emplace_back(Vector3d(1.0, 0.0, 0.0));
	bounding_box.normals.emplace_back(Vector3d(0.0, 1.0, 0.0));
	bounding_box.normals.emplace_back(Vector3d(-1.0, 0.0, 0.0));

	return true;
}


#ifndef _RYSQ_ERI_IMPL_HPP_
#define _RYSQ_ERI_IMPL_HPP_

#include <boost/mpl/int.hpp>
#include <boost/utility/enable_if.hpp>

#include <rysq/eri.hpp>

#include "quartet.hpp"
#include "quadrature.h"
#include "normalize.h"
#include "util.h"
#include "kernels/functor.hpp"
#include "eri-eval.hpp"

#include "cxx/utility/permute.hpp"

namespace mpl = boost::mpl;
namespace meta = rysq::meta;

struct rysq::Eri::Impl {
    virtual ~Impl() {}
    virtual void operator()(const Quartet<Center> &centers,
			    double *I, const Parameters &parameters) = 0;
    template<type A, type B, type C, type D>
    static Impl* instance(const Quartet<Shell> &shells);
};

namespace rysq {
    namespace eri {

	template<class T, class Enable = void>
	struct Kernel;

	// template <type A_, type B_, type C_, type D_>
	// struct Kernel<meta::braket<A_,B_,C_,D_>,
	// 	      typename boost::disable_if<kernels::braket<A_,B_,C_,D_> >::type>
	// {
	//     static const bool value = false;
	// };

	template<class  braket, class enable = void>
	struct enable_if_specialized {};

	template <type A_, type B_, type C_, type D_>
	struct enable_if_specialized<meta::braket<A_,B_,C_,D_>,
				     typename
				     boost::enable_if<kernels::braket<A_,B_,C_,D_>
						      >::type> {
	    typedef void type;
	};

	template <type A_, type B_, type C_, type D_>
	struct Kernel<meta::braket<A_,B_,C_,D_>, /*void*/
		      typename enable_if_specialized<
			  typename meta::sorted<A_,B_,C_,D_>::braket>:: type>
	: Eri::Impl {
	    static const bool value = true;
	    // typedef meta::braket<A_,B_,C_,D_>::braket braket;
	    typedef typename meta::sorted<A_,B_,C_,D_>::braket braket;
	    static const int transpose = meta::sorted<A_,B_,C_,D_>::value;

	    const Quartet<Shell> quartet;

	    Kernel(const Quartet<Shell> &quartet)
		: quartet(quartet.transposed(transpose)) { }

	    static Impl* instance(const Quartet<Shell> &quartet) {
		return new Kernel(quartet);
	    }

	//     static void eval(const Quartet<Shell> &quartet,
	// 		     const Quartet<Center> &centers,
	// 		     const density_set &D6,
	// 		     const fock_set &F6,
	// 		     const Parameters &parameters) {
	// 	double I[braket::size] __attribute__ ((aligned(16))) = { 0.0 };
	// 	double scale = 1.0;
	// 	double cutoff = parameters.cutoff/(5e1*SQRT_4PI5);
	// 	eri::eval(quartet, centers, I, scale, cutoff);
	// 	Parameters p = parameters;
	// 	p.scale *= SQRT_4PI5;
	// 	fock::eval<braket>(D6, F6, I, p);
	//     }
	// };

	    void operator()(const Quartet<Center> &centers,
			    double *I, const Parameters &p) {
		double scale = SQRT_4PI5*p.scale;
		double cutoff = p.cutoff/(5e1*scale);
		eval(quartet, centers, I, scale, cutoff);
	    }

	    static void eval(const Quartet<Shell> &quartet,
			     const Quartet<Center> &centers,
			     double *I, double scale, double cutoff) {
		const double *ri, *rj, *rk, *rl;
		util::unpack(centers, ri, rj, rk, rl);
		cxx::utility::permute(ri, rj, rk, rl, quartet.shuffle_mask);
		eri::eval<braket>(quartet, ri, rj, rk, rl, I, scale, cutoff);
	    }

	};

	template<class braket, class Enable>
	struct Kernel {
	    static const bool value = false;
	    static Eri::Impl* instance(const Quartet<Shell> &quartet) {
		typedef typename braket::A A;
		typedef typename braket::B B;
		typedef typename braket::C C;
		typedef typename braket::D D;

		typedef typename braket::bra bra;
		typedef typename braket::ket ket;
		typedef mpl::int_<(braket::L)/2+1> N;

		typedef mpl::bool_<A::L == 0 && B::L != 0> t0;
		typedef mpl::bool_<C::L == 0 && D::L != 0> t1;
		typedef mpl::bool_<bra::L < ket::L && bra::L == 0> t01;

		// typedef mpl::bool_<A::value < B::value> t0;
		// typedef mpl::bool_<C::value < D::value> t1;
		// typedef mpl::bool_<((A::value == C::value && B::value < D::value) ||
		// 		    (A::value < C::value))> t01;

		typedef meta::state<B::type, A::type> BA_;
		typedef meta::state<A::type, B::type> AB_;
		typedef typename mpl::if_<t0, BA_, AB_>::type bra_;

		typedef meta::state<D::type, C::type> DC_;
		typedef meta::state<C::type, D::type> CD_;
		typedef typename mpl::if_<t1, DC_, CD_>::type ket_;

		typedef typename mpl::if_<t01, ket_, bra_>::type kBra;

		int transpose = Transpose::mask(t0::value, t1::value, t01::value);
		//if (braket::L < 5)std::cout << A::type << B::type << C::type  << D::type << "\n";
		return Kernel<kBra,N>::instance(quartet.transposed(transpose));
	    }
	};

	template<type A, type B, int N>
	struct Kernel<meta::state<A,B>, mpl::int_<N> >
	    : Eri::Impl, Quadrature::Transform {

	    typedef meta::state<A,B> bra;

	    const Quartet<Shell> quartet;
	    Quadrature::Primitives primitives;
	    Parameters parameters;
	    double scale, *eri;

	    Kernel(const Quartet<Shell> &quartet)
		: quartet(quartet),
		  primitives(this->quartet) {
	    }

	    static Eri::Impl* instance(const Quartet<Shell> &quartet) {
		return new Kernel(quartet);
	    }

	    void operator()(const Quartet<Center> &centers,
			    double *eri, const Parameters &parameters) {
		this->eri = eri;
		this->scale = parameters.scale*SQRT_4PI5;
		 double scale_ = 1;
		double cutoff_ = parameters.cutoff/(5e1*scale);
		Quadrature::eval<bra,N>(quartet, centers, scale_, cutoff_,
					primitives, *this);
	    }

	    struct Transform {
		static const int size = bra::size;
		double  scale, *eri;
		Transform(double scale,double *eri) : scale(scale), eri(eri) {}
		void operator()(int k, int l, int kl, const double *eri) {
		    // std::cout << kl << std::endl;
		    double *eri_ = this->eri + kl*size;
		    for (int i = 0; i < size; ++i) {
		        eri_[i] = scale*eri[i];
			// std::cout << eri[i] << std::endl;
		    }
		}
	    };

	    void operator()(const Quadrature::Primitives &primitives) {
		int K = primitives.K;
 		if (K == 0) return;
		const Shell &c = quartet[2];
		const Shell &d = quartet[3];
		Transform transform( scale,this->eri);
		// throw;
		Quadrature::apply<bra,N>(c, d, primitives, transform);
	    }

	};

	// template <type A, type B, int N>
	// struct Kernel<meta::state<A,B>,  mpl::int_< N> >
	// : rysq::Eri::Impl, Quadrature::Transform {

	// };

    }
}

namespace rysq {

    template<type A, type B, type C, type D>
    inline Eri::Impl* Eri::Impl::instance(const Quartet<Shell> &quartet) {
	typedef meta::braket<A,B,C,D> braket;
	return eri::Kernel<braket>::instance(quartet);
    }

}

#endif /* _RYSQ_ERI_HPP_ */


/*
 * Copyright (c) 2021 Elastos Foundation
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "Mnemonic.h"
#include <utf8proc.h>

#include <Common/hash.h>
#include <Common/ErrorChecker.h>
#include <Common/uint256.h>
#include <Common/Utils.h>

#include <sstream>
#include <boost/algorithm/string.hpp>

namespace fs = boost::filesystem;

namespace Elastos {
    namespace ElaWallet {

        Mnemonic::Mnemonic() {
        }

        WordList Mnemonic::Create(const bytes_t &entropy, const Dictionary &lexicon) {
            if ((entropy.size() % MnemonicSeedMultiple) != 0)
                return {};

            const size_t entropyBits = (entropy.size() * ByteBits);
            const size_t checkBits = (entropyBits / EntropyBitDivisor);
            const size_t totalBits = (entropyBits + checkBits);
            const size_t wordCount = (totalBits / BitsPerWord);

            if ((totalBits % BitsPerWord) != 0 || (wordCount % MnemonicWordMultiple) != 0)
                return {};

            bytes_t data = entropy + sha256(entropy);

            size_t bit = 0;
            WordList words;

            for (size_t word = 0; word < wordCount; word++) {
                size_t position = 0;
                for (size_t loop = 0; loop < BitsPerWord; loop++) {
                    bit = (word * BitsPerWord + loop);
                    position <<= 1;

                    const auto byte = bit / ByteBits;

                    if ((data[byte] & Bip39Shift(bit)) > 0)
                        position++;
                }

                if (position >= DictionarySize)
                    return {};
                words.push_back(lexicon[position]);
            }

            if (words.size() != ((bit + 1) / BitsPerWord))
                return {};

            return words;
        }

        bytes_t Mnemonic::Entropy(const WordList &words, const Dictionary &lexicon) {
            const auto wordCount = words.size();
            if ((wordCount % MnemonicWordMultiple) != 0)
                return {};

            const auto totalBits = BitsPerWord * wordCount;
            const auto checkBits = totalBits / (EntropyBitDivisor + 1);
            const auto entropyBits = totalBits - checkBits;

            if ((entropyBits % ByteBits) != 0)
                return {};

            size_t bit = 0;
            bytes_t data((totalBits + ByteBits - 1) / ByteBits, 0);

            for (const auto& word: words) {
                const auto position = FindPosition(lexicon, word);
                if (position == -1)
                    return {};

                for (size_t loop = 0; loop < BitsPerWord; loop++, bit++)
                {
                    if (position & (1 << (BitsPerWord - loop - 1)))
                    {
                        const auto byte = bit / ByteBits;
                        data[byte] |= Bip39Shift(bit);
                    }
                }
            }

            data.resize(entropyBits / ByteBits);
            return data;
        }

        bool Mnemonic::Validate(const WordList &words, const Dictionary &lexicon) {
            bytes_t entropy = Entropy(words, lexicon);
            if (entropy.empty())
                return false;

            const auto mnemonic = Create(entropy, lexicon);
            return std::equal(mnemonic.begin(), mnemonic.end(), words.begin());
        }

        bool Mnemonic::Validate(const WordList &words, const DictionaryMap &lexicons) {
            for (const auto &lexicon : lexicons) {
                if (Validate(words, *lexicon.second))
                    return true;
            }

            return false;
        }

        std::string Mnemonic::Create(const std::string &language, WordCount wordCount) {
            std::string languageLowerCase = language;
            size_t entropyBits = 0;

            switch (wordCount) {
                case WORDS_12: entropyBits = 128; break;
                case WORDS_15: entropyBits = 160; break;
                case WORDS_18: entropyBits = 192; break;
                case WORDS_21: entropyBits = 224; break;
                case WORDS_24: entropyBits = 256; break;
                default:
                    ErrorChecker::ThrowParamException(Error::InvalidMnemonicWordCount, "invalid mnemonic word count");
            }

            std::transform(languageLowerCase.begin(), languageLowerCase.end(), languageLowerCase.begin(), ::tolower);
            bytes_t entropy = Utils::GetRandom(entropyBits / 8);

            auto it = Language::All.find(languageLowerCase);
            if (it == Language::All.end())
                ErrorChecker::ThrowParamException(Error::InvalidArgument, "invalid mnemonic language");

            WordList words = Create(entropy, *it->second);
            return boost::algorithm::join(words, " ");
        }

        bytes_t Mnemonic::Entropy(const std::string &mnemonic, const Dictionary &lexicon) {
            utf8proc_uint8_t *mnemonicTmp = utf8proc_NFKD((const utf8proc_uint8_t *)mnemonic.c_str());
            std::string mnemonicFixed((char *)mnemonicTmp);
            free(mnemonicTmp);

            WordList words;
            boost::algorithm::split(words, mnemonicFixed, boost::is_any_of(" \n\r\t"), boost::token_compress_on);
            words.erase(std::remove(words.begin(), words.end(), ""), words.end());

            return Entropy(words, lexicon);
        }

        bool Mnemonic::Validate(const std::string &mnemonic) {
            utf8proc_uint8_t *mnemonicTmp = utf8proc_NFKD((const utf8proc_uint8_t *)mnemonic.c_str());
            std::string mnemonicFixed((char *)mnemonicTmp);
            free(mnemonicTmp);

            WordList words;
            boost::algorithm::split(words, mnemonicFixed, boost::is_any_of(" \n\r\t"), boost::token_compress_on);
            words.erase(std::remove(words.begin(), words.end(), ""), words.end());

            return Validate(words);
        }

        uint512 Mnemonic::DeriveSeed(const std::string &mnemonic, const std::string &passphrase) {
            utf8proc_uint8_t *mnemonicTmp = utf8proc_NFKD((const utf8proc_uint8_t *)mnemonic.c_str());
            std::string mnemonicFixed((char *)mnemonicTmp);
            free(mnemonicTmp);

            utf8proc_uint8_t *passphraseTmp = utf8proc_NFKD((const utf8proc_uint8_t *)passphrase.c_str());
            std::string passphraseFixed((char *)passphraseTmp);
            free(passphraseTmp);

            WordList words;
            boost::algorithm::split(words, mnemonicFixed, boost::is_any_of(" \n\r\t"), boost::token_compress_on);
            words.erase(std::remove(words.begin(), words.end(), ""), words.end());

            ErrorChecker::CheckLogic(!Validate(words), Error::Mnemonic, "invalid mnemonic");

            std::string sentence = boost::algorithm::join(words, " ");
            std::string salt = "mnemonic" + passphraseFixed;

            return PBKDF2(bytes_t(sentence.data(), sentence.size()), bytes_t(salt.data(), salt.size()), 2048);
        }

        uint512 Mnemonic::PBKDF2(const bytes_t &pw, const bytes_t &salt, unsigned int rounds) {
            bytes_t s(salt.size() + sizeof(uint32_t));
            uint32_t i, j;
            uint512 key;
            size_t length, keyLen = key.size();
            bytes_t U, T, k;

            assert(rounds > 0);

            memcpy(s.data(), salt.data(), salt.size());

            for (i = 0; keyLen > 0; i++) {
                s[salt.size() + 0] = (uint8_t)(((i + 1) >> 24) & 0xff);
                s[salt.size() + 1] = (uint8_t)(((i + 1) >> 16) & 0xff);
                s[salt.size() + 2] = (uint8_t)(((i + 1) >> 8) & 0xff);
                s[salt.size() + 3] = (uint8_t)((i + 1) & 0xff);

                U = hmac_sha512(pw, s); // U1 = hmac_hash(pw, salt || be32(i))
                T = U;

                for (unsigned int r = 1; r < rounds; r++) {
                    U = hmac_sha512(pw, U); // Urounds = hmac_hash(pw, Urounds-1)
                    for (j = 0; j < T.size(); j++) T[j] ^= U[j]; // Ti = U1 ^ U2 ^ ... ^ Urounds
                }

                // dk = T1 || T2 || ... || Tdklen/hlen
                length = keyLen < T.size() ? keyLen : T.size();
                k += T;
                keyLen -= length;
            }

            key = uint512(k);

            s.clean();
            U.clean();
            T.clean();
            k.clean();

            return key;
        }

    }
}


// Boost.Geometry

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

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

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

#include "common.hpp"

#include <boost/geometry/algorithms/distance.hpp>

int test_main(int, char*[])
{
    geom g;

    bg::distance(g.pt, g.pt, bg::strategy::distance::pythagoras<>());
    bg::distance(g.pt, g.pt, bg::strategy::distance::haversine<>());
    bg::distance(g.pt, g.pt, bg::strategy::distance::geographic<>());
    bg::distance(g.pt, g.mpt, bg::strategy::distance::pythagoras<>());
    bg::distance(g.pt, g.mpt, bg::strategy::distance::haversine<>());
    bg::distance(g.pt, g.mpt, bg::strategy::distance::geographic<>());
    bg::distance(g.mpt, g.mpt, bg::strategy::distance::pythagoras<>());
    bg::distance(g.mpt, g.mpt, bg::strategy::distance::haversine<>());
    bg::distance(g.mpt, g.mpt, bg::strategy::distance::geographic<>());
    bg::distance(g.pt, g.ls, bg::strategy::distance::projected_point<>());
    bg::distance(g.pt, g.ls, bg::strategy::distance::cross_track<>());
    bg::distance(g.pt, g.ls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.pt, g.mls, bg::strategy::distance::projected_point<>());
    bg::distance(g.pt, g.mls, bg::strategy::distance::cross_track<>());
    bg::distance(g.pt, g.mls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpt, g.ls, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpt, g.ls, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpt, g.ls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpt, g.mls, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpt, g.mls, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpt, g.mls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.ls, g.ls, bg::strategy::distance::projected_point<>());
    bg::distance(g.ls, g.ls, bg::strategy::distance::cross_track<>());
    bg::distance(g.ls, g.ls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.ls, g.mls, bg::strategy::distance::projected_point<>());
    bg::distance(g.ls, g.mls, bg::strategy::distance::cross_track<>());
    bg::distance(g.ls, g.mls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mls, g.mls, bg::strategy::distance::projected_point<>());
    bg::distance(g.mls, g.mls, bg::strategy::distance::cross_track<>());
    bg::distance(g.mls, g.mls, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.pt, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.pt, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.pt, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.pt, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.pt, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.pt, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.pt, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.pt, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.pt, g.mpo, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpt, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpt, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpt, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpt, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpt, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpt, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpt, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpt, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpt, g.mpo, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.ls, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.ls, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.ls, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.ls, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.ls, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.ls, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.ls, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.ls, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.ls, g.mpo, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mls, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.mls, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.mls, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mls, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.mls, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.mls, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mls, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.mls, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.mls, g.mpo, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.r, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.r, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.r, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.r, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.r, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.r, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.r, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.r, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.r, g.mpo, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.po, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.po, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.po, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.po, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.po, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.po, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.po, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.po, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.po, g.mpo, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpo, g.r, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpo, g.r, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpo, g.r, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpo, g.po, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpo, g.po, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpo, g.po, bg::strategy::distance::geographic_cross_track<>());
    bg::distance(g.mpo, g.mpo, bg::strategy::distance::projected_point<>());
    bg::distance(g.mpo, g.mpo, bg::strategy::distance::cross_track<>());
    bg::distance(g.mpo, g.mpo, bg::strategy::distance::geographic_cross_track<>());

    return 0;
}


#pragma once

#include <memory>
#include <utility>

#include <boost/operators.hpp>

#include <elle/attribute.hh>
#include <elle/operator.hh>
#include <elle/serialization.hh>

#include <elle/cryptography/fwd.hh>
#include <elle/cryptography/types.hh>
#include <elle/cryptography/Oneway.hh>
#include <elle/cryptography/Cipher.hh>
#if defined(ELLE_CRYPTOGRAPHY_ROTATION)
# include <elle/cryptography/rsa/Seed.hh>
#endif
#include <elle/cryptography/rsa/Padding.hh>
#include <elle/cryptography/rsa/defaults.hh>

namespace elle
{
  namespace cryptography
  {
    namespace rsa
    {
      /// Represent a public key in the RSA asymmetric cryptosystem.
      class PublicKey
        : public elle::Printable
        , public std::enable_shared_from_this<PublicKey>
        , private boost::totally_ordered<PublicKey>
      {
      /*-------------.
      | Construction |
      `-------------*/
      public:
        /// Construct a public key out of its private counterpart.
        explicit
        PublicKey(PrivateKey const& k);
        /// Construct a public key based on the given EVP_PKEY key whose
        /// ownership is transferred.
        explicit
        PublicKey(::EVP_PKEY* key);
        /// Construct a public key based on the given RSA key whose
        /// ownership is transferred to the public key.
        explicit
        PublicKey(::RSA* rsa);
        PublicKey(PublicKey const& other);
        PublicKey(PublicKey&& other);
        virtual
        ~PublicKey() = default;

        /*--------.
        | Methods |
        `--------*/
      private:
        /// Check that the key is valid.
        void
        _check() const;
      public:
        /// Encrypt the plain text and return the ciphered text in an envelope.
        virtual
        elle::Buffer
        seal(elle::ConstWeakBuffer const& plain,
             Cipher const cipher = defaults::envelope_cipher,
             Mode const mode = defaults::envelope_mode) const;
        /// Encrypt the stream-based plain text and seal it in an envelope.
        void
        seal(std::istream& plain,
             std::ostream& code,
             Cipher const cipher = defaults::envelope_cipher,
             Mode const mode = defaults::envelope_mode) const;
        /// Encrypt a plain text using the raw public key.
        ///
        /// WARNING: This method cannot be used to encrypt large amount of
        ///          data as constrained by the key's modulus. Please refer
        ///          to the seal()/open() methods.
        virtual
        elle::Buffer
        encrypt(elle::ConstWeakBuffer const& plain,
                Padding const padding = defaults::encryption_padding) const;
        /// Verify the given signature against the original plain text.
        bool
        verify(elle::ConstWeakBuffer const& signature,
               elle::ConstWeakBuffer const& plain,
               Padding const padding = defaults::signature_padding,
               Oneway const oneway = defaults::oneway) const;
        /// Verify the given signature against the object
        template <typename T>
        bool
        verify(elle::ConstWeakBuffer const& signature, T const& o) const;
        template <typename T>
        std::function<bool ()>
        verify_async(elle::ConstWeakBuffer const& signature, T const& o) const;
      private:
        virtual
        bool
        _verify(elle::ConstWeakBuffer const& signature,
                elle::ConstWeakBuffer const& plain,
                Padding const padding = defaults::signature_padding,
                Oneway const oneway = defaults::oneway) const;
        template <typename T>
        std::pair<elle::Buffer, elle::Buffer>
        _verify_data(elle::ConstWeakBuffer const& signature,
                     T const& o) const;
      public:
        /// Whether the given signature matches the stream-based plain.
        bool
        verify(elle::ConstWeakBuffer const& signature,
               std::istream& plain,
               Padding const padding = defaults::signature_padding,
               Oneway const oneway = defaults::oneway) const;
        /// Return the public key's size in bytes.
        uint32_t
        size() const;
        /// Return the public key's length in bits.
        uint32_t
        length() const;
      private:
        bool
        _verify(elle::ConstWeakBuffer const& signature,
                std::istream& plain,
                Padding const padding = defaults::signature_padding,
                Oneway const oneway = defaults::oneway) const;

# if defined(ELLE_CRYPTOGRAPHY_ROTATION)
        /*---------.
        | Rotation |
        `---------*/
      public:
        /// Construct a public key based on a given seed i.e in a deterministic
        /// way.
        explicit
        PublicKey(Seed const& seed);
        /// Return the seed once unrotated by the public key.
        Seed
        unrotate(Seed const& seed) const;
# endif

        /*----------.
        | Operators |
        `----------*/
      public:
        bool
        operator ==(PublicKey const& other) const;
        bool
        operator <(PublicKey const& other) const;
        PublicKey&
        operator =(PublicKey&& other) = default;

        /*----------.
        | Printable |
        `----------*/
      public:
        void
        print(std::ostream& stream) const override;

        /*--------------.
        | Serialization |
        `--------------*/
      public:
        PublicKey(elle::serialization::SerializerIn& serializer);
        void
        serialize(elle::serialization::Serializer& serializer);
        using serialization_tag = elle::serialization_tag;

        /*-----------.
        | Attributes |
        `-----------*/
      public:
        ELLE_ATTRIBUTE_R(types::EVP_PKEY, key);
      };

      namespace _details
      {
        void
        raise(std::string const& message);
        types::EVP_PKEY
        build_evp(::RSA* rsa);
      }
    }
  }
}

//
// ---------- DER -------------------------------------------------------------
//

namespace elle
{
  namespace cryptography
  {
    namespace rsa
    {
      namespace publickey
      {
        namespace der
        {
          /*----------.
          | Functions |
          `----------*/

          /// Encode the public key in DER.
          elle::Buffer
          encode(PublicKey const& K);
          /// Decode the public key from a DER representation.
          PublicKey
          decode(elle::ConstWeakBuffer const& buffer);
        }
      }
    }
  }
}

namespace std
{
  template <>
  struct hash<elle::cryptography::rsa::PublicKey>
  {
    size_t
    operator ()(elle::cryptography::rsa::PublicKey const& value) const;
  };
}

#include <elle/cryptography/rsa/PublicKey.hxx>


#include "Common/Common.h"
#include "Demos/Visualization/MiniGL.h"
#include "Demos/Visualization/Selection.h"
#include "GL/glut.h"
#include "Simulation/TimeManager.h"
#include <Eigen/Dense>
#include "Simulation/SimulationModel.h"
#include "Simulation/TimeStepController.h"
#include <iostream>
#include "Utils/OBJLoader.h"
#include "Demos/Visualization/Visualization.h"
#include "Utils/Logger.h"
#include "Utils/Timing.h"
#include "Utils/FileSystem.h"
#include "Demos/Common/DemoBase.h"
#include "Demos/Common/TweakBarParameters.h"
#include "Simulation/Simulation.h"

#define _USE_MATH_DEFINES

#include "math.h"

// Enable memory leak detection
#if defined(_DEBUG) && !defined(EIGEN_ALIGN)
#define new DEBUG_NEW
#endif

using namespace PBD;
using namespace Eigen;
using namespace std;
using namespace Utilities;

void initParameters();
void timeStep();
void buildModel();
void createBodyModel();
void render();
void reset();

DemoBase *base;

const int numberOfBodies = 10;
const Real width = static_cast<Real>(1.0);
const Real height = static_cast<Real>(0.1);
const Real depth = static_cast<Real>(0.1);

// main
int main(int argc, char **argv)
{
    REPORT_MEMORY_LEAKS

    base = new DemoBase();
    base->init(argc, argv, "Rigid body demo");

    SimulationModel *model = new SimulationModel();
    model->init();
    Simulation::getCurrent()->setModel(model);

    buildModel();

    initParameters();

    Simulation::getCurrent()->setSimulationMethodChangedCallback([&]()
                                                                 {
                                                                     reset();
                                                                     initParameters();
                                                                     base->getSceneLoader()->readParameterObject(Simulation::getCurrent()->getTimeStep());
                                                                 });

    // OpenGL
    MiniGL::setClientIdleFunc(50, timeStep);
    MiniGL::setKeyFunc(0, 'r', reset);
    MiniGL::setClientSceneFunc(render);
    MiniGL::setViewport(40.0, 0.1f, 500.0, Vector3r(5.0, 10.0, 30.0), Vector3r(5.0, 0.0, 0.0));

    glutMainLoop();

    Utilities::Timing::printAverageTimes();
    Utilities::Timing::printTimeSums();

    delete Simulation::getCurrent();
    delete base;
    delete model;

    return 0;
}

void initParameters()
{
    TwRemoveAllVars(MiniGL::getTweakBar());
    TweakBarParameters::cleanup();

    MiniGL::initTweakBarParameters();

    TweakBarParameters::createParameterGUI();
    TweakBarParameters::createParameterObjectGUI(base);
    TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent());
    TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getModel());
    TweakBarParameters::createParameterObjectGUI(Simulation::getCurrent()->getTimeStep());
}

void reset()
{
    Utilities::Timing::printAverageTimes();
    Utilities::Timing::reset();

    Simulation::getCurrent()->reset();
    base->getSelectedParticles().clear();
}

void timeStep()
{
    const Real pauseAt = base->getValue<Real>(DemoBase::PAUSE_AT);
    if ((pauseAt > 0.0) && (pauseAt < TimeManager::getCurrent()->getTime()))
        base->setValue(DemoBase::PAUSE, true);

    if (base->getValue<bool>(DemoBase::PAUSE))
        return;

    // Simulation code
    SimulationModel *model = Simulation::getCurrent()->getModel();
    const unsigned int numSteps = base->getValue<unsigned int>(DemoBase::NUM_STEPS_PER_RENDER);
    for (unsigned int i = 0; i < numSteps; i++)
    {
        START_TIMING("SimStep");
        Simulation::getCurrent()->getTimeStep()->step(*model);
        STOP_TIMING_AVG;
    }
}

void buildModel()
{
    TimeManager::getCurrent()->setTimeStepSize(static_cast<Real>(0.005));

    createBodyModel();
}

void render()
{
    base->render();
}

void loadObj(const std::string &filename, VertexData &vd, IndexedFaceMesh &mesh, const Vector3r &scale)
{
    std::vector<OBJLoader::Vec3f> x;
    std::vector<OBJLoader::Vec3f> normals;
    std::vector<OBJLoader::Vec2f> texCoords;
    std::vector<MeshFaceIndices> faces;
    OBJLoader::Vec3f s = {(float) scale[0], (float) scale[1], (float) scale[2]};
    OBJLoader::loadObj(filename, &x, &faces, &normals, &texCoords, s);

    mesh.release();
    const unsigned int nPoints = (unsigned int) x.size();
    const unsigned int nFaces = (unsigned int) faces.size();
    const unsigned int nTexCoords = (unsigned int) texCoords.size();
    mesh.initMesh(nPoints, nFaces * 2, nFaces);
    vd.reserve(nPoints);
    for (unsigned int i = 0; i < nPoints; i++)
    {
        vd.addVertex(Vector3r(x[i][0], x[i][1], x[i][2]));
    }
    for (unsigned int i = 0; i < nTexCoords; i++)
    {
        mesh.addUV(texCoords[i][0], texCoords[i][1]);
    }
    for (unsigned int i = 0; i < nFaces; i++)
    {
        // Reduce the indices by one
        int posIndices[3];
        int texIndices[3];
        for (int j = 0; j < 3; j++)
        {
            posIndices[j] = faces[i].posIndices[j] - 1;
            if (nTexCoords > 0)
            {
                texIndices[j] = faces[i].texIndices[j] - 1;
                mesh.addUVIndex(texIndices[j]);
            }
        }

        mesh.addFace(&posIndices[0]);
    }
    mesh.buildNeighbors();

    mesh.updateNormals(vd, 0);
    mesh.updateVertexNormals(vd);

    LOG_INFO << "Number of triangles: " << nFaces;
    LOG_INFO << "Number of vertices: " << nPoints;
}

/** Create the rigid body model
*/
void createBodyModel()
{
    SimulationModel *model = Simulation::getCurrent()->getModel();
    SimulationModel::RigidBodyVector &rb = model->getRigidBodies();
    SimulationModel::ConstraintVector &constraints = model->getConstraints();

    string fileName = FileSystem::normalizePath(base->getDataPath() + "/models/cube.obj");
    IndexedFaceMesh mesh;
    VertexData vd;
    loadObj(fileName, vd, mesh, Vector3r(width, height, depth));

    string fileName2 = FileSystem::normalizePath(base->getDataPath() + "/models/bunny_10k.obj");
    IndexedFaceMesh mesh2;
    VertexData vd2;
    loadObj(fileName2, vd2, mesh2, Vector3r(2.0, 2.0, 2.0));

    rb.resize(numberOfBodies);
    const Real density = 1.0;
    for (unsigned int i = 0; i < numberOfBodies - 1; i++)
    {
        rb[i] = new RigidBody();
        rb[i]->initBody(density, Vector3r((Real) i * width, 0.0, 0.0), Quaternionr(1.0, 0.0, 0.0, 0.0), vd, mesh);
    }
    // Make first body static
    rb[0]->setMass(0.0);

    // bunny
    const Quaternionr q(AngleAxisr(static_cast<Real>(1.0 / 6.0 * M_PI), Vector3r(0.0, 0.0, 1.0)));
    const Vector3r t(static_cast<Real>(0.411) + (static_cast<Real>(numberOfBodies) - static_cast<Real>(1.0)) * width, static_cast<Real>(-1.776),
                     static_cast<Real>(0.356));
    rb[numberOfBodies - 1] = new RigidBody();
    rb[numberOfBodies - 1]->initBody(density, t, q, vd2, mesh2);

    constraints.reserve(numberOfBodies - 1);
    for (unsigned int i = 0; i < numberOfBodies - 1; i++)
    {
        model->addBallJoint(i, i + 1, Vector3r((Real) i * width + static_cast<Real>(0.5) * width, 0.0, 0.0));
    }
}


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

#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/partition.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/vector.hpp>
namespace mpl = boost::mpl;

struct is_even {
    template <typename N>
    using apply = mpl::integral_c<bool, N::type::value % 2 == 0>;
};

using vector = <%= mpl_vector((1..input_size).to_a.map { |n|
    "mpl::integral_c<int, #{n}>"
}) %>;

using result = mpl::partition<vector, is_even>::type;

int main() { }


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <ros/ros.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <urdf_model/model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_scene/planning_scene.h>
#include "sdf_tools/sdf.hpp"
#include "sdf_tools/SDF.h"

#ifndef SDF_BUILDER_HPP
#define SDF_BUILDER_HPP

namespace sdf_tools
{
    static const uint8_t USE_CACHED = 0x00;
    static const uint8_t USE_ONLY_OCTOMAP = 0x01;
    static const uint8_t USE_ONLY_COLLISION_OBJECTS = 0x02;
    static const uint8_t USE_FULL_PLANNING_SCENE = 0x03;

    typedef struct
    {
        uint32_t location[3];
        uint32_t closest_point[3];
        double distance_square;
        int32_t update_direction;
    } bucket_cell;

    typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;

    inline double ComputeDistanceSquared(int32_t x1, int32_t y1, int32_t z1, int32_t x2, int32_t y2, int32_t z2)
    {
        int32_t dx = x1 - x2;
        int32_t dy = y1 - y2;
        int32_t dz = z1 - z2;
        return double((dx * dx) + (dy * dy) + (dz * dz));
    }

    class SDF_Builder
    {
    protected:

        bool initialized_;
        bool has_cached_sdf_;
        bool has_cached_collmap_;
        bool has_planning_scene_;
        Eigen::Affine3d origin_transform_;
        std::string frame_;
        double x_size_;
        double y_size_;
        double z_size_;
        double resolution_;
        float OOB_value_;
        SignedDistanceField cached_sdf_;
        VoxelGrid::VoxelGrid<uint8_t> cached_collmap_;
        std::shared_ptr<planning_scene::PlanningScene> planning_scene_ptr_;
        ros::NodeHandle nh_;
        ros::ServiceClient planning_scene_client_;

        SignedDistanceField UpdateSDFFromPlanningScene();

        VoxelGrid::VoxelGrid<uint8_t> UpdateCollisionMapFromPlanningScene();

        bool BuildInternalPlanningScene();

        DistanceField BuildDistanceField(std::vector<Eigen::Vector3i>& points);

        std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods();

        inline int GetDirectionNumber(int dx, int dy, int dz)
        {
            return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
        }

        std::string GenerateSDFComputeBotURDFString();

        std::string GenerateSDFComputeBotSRDFString();

    public:

        SDF_Builder(ros::NodeHandle& nh, Eigen::Affine3d origin_transform, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service);

        SDF_Builder(ros::NodeHandle& nh, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service);

        SDF_Builder()
        {
            initialized_ = false;
            has_cached_sdf_ = false;
            has_cached_collmap_ = false;
            has_planning_scene_ = false;
        }

        void UpdatePlanningSceneFromMessage(moveit_msgs::PlanningScene& planning_scene);

        SignedDistanceField UpdateSDF(uint8_t update_mode);

        SignedDistanceField GetCachedSDF();

        VoxelGrid::VoxelGrid<uint8_t> UpdateCollisionMap(uint8_t update_mode);

        VoxelGrid::VoxelGrid<uint8_t> GetCachedCollisionMap();

    };

}

#endif // SDF_BUILDER_HPP