/*
 * equiv_quiver_matrix.cc
 * Copyright 2014-2015 John Lawson
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include "equiv_quiver_matrix.h"

#include <algorithm>
#include <boost/functional/hash.hpp>

#include "array_utils.h"
#include "equivalence_checker.h"

namespace cluster {
	EquivQuiverMatrix::EquivQuiverMatrix()
		: QuiverMatrix()
		, rows_()
		, checker_(EquivalenceChecker::Get(0))
		, sorted_rows_()
	{}

	EquivQuiverMatrix::EquivQuiverMatrix(const int rows, const int cols)
		: QuiverMatrix(rows, cols)
		, rows_(rows, std::make_pair(0,0))
		, checker_(EquivalenceChecker::Get(rows))
		, sorted_rows_(rows_)
	{}

	EquivQuiverMatrix::EquivQuiverMatrix(const int rows, const int cols,
			const int values[])
		: QuiverMatrix(rows, cols, values)
		, rows_(rows, std::make_pair(0,0))
		, checker_(EquivalenceChecker::Get(rows))
		, sorted_rows_(rows_)
	{
		reset();
	}

	EquivQuiverMatrix::EquivQuiverMatrix(const IntMatrix& matrix)
		: QuiverMatrix(matrix)
		, rows_(matrix.num_rows(), std::make_pair(0,0))
		, checker_(EquivalenceChecker::Get(matrix.num_rows()))
		, sorted_rows_(rows_)
	{
		reset();
	}

	EquivQuiverMatrix::EquivQuiverMatrix(std::string const& str)
		: QuiverMatrix(str)
		, rows_(num_rows(), std::make_pair(0,0))
		, checker_(EquivalenceChecker::Get(num_rows_))
		, sorted_rows_(rows_)
	{
		reset();
	}

	void EquivQuiverMatrix::set_matrix(const IntMatrix& mat) {
		if(mat.num_rows() != num_rows_) {
			rows_ = PairVector(mat.num_rows(), std::make_pair(0,0));
			sorted_rows_ = rows_;
		}
		IntMatrix::set_matrix(mat);
		reset();
	}

	/* Note that the static_cast will create a copy of mat */
	bool EquivQuiverMatrix::equals(const IntMatrix &mat) const {
		auto e_mat = static_cast<EquivQuiverMatrix>(mat);
		return checker_->are_equivalent(*this, e_mat);
	}

	bool EquivQuiverMatrix::equals(const EquivQuiverMatrix &mat) const {
		return checker_->are_equivalent(*this, mat);
	}

	void EquivQuiverMatrix::reset() {
		for(size_t i = 0; i < rows_.size(); ++i) {
			rows_[i].first = 0;
			rows_[i].second = 0;
		}
		int row_i = 0;
		int col_i = 0;
		int i = 0;
		while(i < num_rows_ * num_cols_) {
			rows_[row_i].first += data_[i];
			rows_[row_i].second += std::abs(data_[i]);

			i++;
			col_i++;
			if(col_i == num_cols_) {
				col_i = 0;
				row_i++;
			}
		}
		IntMatrix::reset();
	}

	EquivQuiverMatrix::CPermutation
	EquivQuiverMatrix::get_permutation(const IntMatrix& mat) const {
		this->equals(mat);
		return checker_->last_row_map();
	}

	EquivQuiverMatrix::PermVecPtr
	EquivQuiverMatrix::all_permutations(const EquivQuiverMatrix& mat) const {
		return checker_->valid_row_maps(*this, mat);
	}

	/* Private methods */

	std::size_t EquivQuiverMatrix::compute_hash() const {
		std::size_t hash = 137;
		sorted_rows_ = rows_;
		std::sort(sorted_rows_.begin(), sorted_rows_.end());
		boost::hash_combine(hash, arrays::hash(sorted_rows_));

		return hash;
	}
}


/*
 * Copyright (c) 2017, <copyright holder> <email>
 * 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.
 *     * Neither the name of the <organization> nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY <copyright holder> <email> ''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 <copyright holder> <email> 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.
 *
 */

#include "predeal.h"
#include<fstream>
#include <boost/concept_check.hpp>
using namespace std;
void predeal::readpcdfile(const string& dir, pcl::PointCloud< pcl::PointXYZ >& pcd)
{
  ifstream file;
  file.open(dir.c_str(), ios_base::in);
  if(file.good())
  {
      while(file.good())
      {
	pcl::PointXYZ tempoint;
	file>>tempoint.x>>tempoint.y>>tempoint.z;
	pcd.push_back(tempoint);
      }
  }
  else
  {
    cout<<"can't open the file in the specified dir"<<endl;
    assert(file.good());
  }
}

void predeal::readposefile(const string& dir, vector<Eigen::Vector3f> &v_pose)
{

  ifstream file;
  file.open(dir.c_str(),ios_base::in);
  int count_f=0;
  if(file.good())
  {
      while(file.good())
      {
       Eigen::Vector3f tempose;
       if(count_f==0)
       {
	 file>>tempose(0);
	 count_f++;
	 if(file.eof())
	 {
	   cout<<"文件为空"<<endl;
	   file.close();
	  }
	  else
	  {
	    file>>tempose(1)>>tempose(2);
	  }
       }
       else
       {
	   file>>tempose(0)>>tempose(1)>>tempose(2);
       }
       v_pose.push_back(tempose);
      }
  }
  else
  {
    cout<<"can't open the file in the specified dirctory"<<endl;
    assert(file.good());
  }
  file.close();
}

void predeal::vector3ftoRationTtrans(const Eigen::Vector3f& pose, Eigen::Matrix3f& R, Eigen::Vector3f& t)
{
   Eigen::Quaternion<float> q_w_target(cos(pose(2)/2),0,0,sin(pose(2)/2));
   t(0)=pose(0);
   t(1)=pose(1);
   t(2)=0.0f;
  R=q_w_target.toRotationMatrix();
}

Eigen::Vector3f predeal::computeConstraint( const Eigen::Vector3f tgt_pose,const Eigen::Vector3f& src_pose)
{
	Eigen::Quaternion<float> q_w_target(cos(tgt_pose(2)/2),0,0,sin(tgt_pose(2)/2));
	Eigen::Quaternion<float> q_w_source(cos(src_pose(2)/2),0,0,sin(src_pose(2)/2));
	Eigen::Vector3f t_w_target, t_w_source;
	t_w_target<<tgt_pose(0), tgt_pose(1),0.0f;
	t_w_source<<src_pose(0), src_pose(1),0.0f;
	Eigen::Matrix3f R_initial=q_w_target.toRotationMatrix().transpose()*q_w_source.toRotationMatrix();
	Eigen::Vector3f t_initial=q_w_target.toRotationMatrix().transpose()*(t_w_source-t_w_target);
	cout<<t_initial<<endl;
	Eigen::Quaternion<float> q(R_initial);
        Eigen::Vector3f v;
	//以下的处理非常重要，只适用于只存在偏航角的情况
	float a1=acos(q.w());
	float a2=-a1;
	if(abs(sin(a1)-q.z())<0.001f)
	{
	  v(2)=2*a1;

	}
	else if(abs(sin(a2)-q.z())<0.001f)
	{
	  v(2)=2*a2;
	  //cout<<v(2)<<endl;
	}
	else{
	  cout<<"四元数解析错误，该旋转轴不为z轴"<<endl;
	  exit(0);
	}

    v(1)=t_initial(1);
    v(0)=t_initial(0);
    return v;
}


#pragma once

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <image_geometry/pinhole_camera_model.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <jsk_recognition_msgs/Rect.h>

// #include <sensor_msgs/Point_cloud2.h>
#include <jsk_recognition_msgs/BoundingBox.h>

// opencv.
// #include <opencv/cv.h>
#include <opencv2/opencv.hpp>
// #include <opencv2/Core/core.hpp>
#include <cv_bridge/cv_bridge.h>
#include "opencv2/objdetect.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/videoio.hpp"
#include <iostream>
// pcl.
// #include <pcl/pcl.h>

//! Eigen
#include <Eigen/Dense>

//! user
#include "Utility.hpp"

using namespace std;
using namespace Eigen;

//http://wiki.ros.org/image_transport/Tutorials/SubscribingToImages
//http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage

namespace bluerov_detection_tracking
{

class CascadeDetection
{

public:
    // Registration(ros::NodeHandle& node);
    // virtual ~Registration();
    CascadeDetection(ros::NodeHandle& node);
    virtual ~CascadeDetection();

    bool readParameters();

    bool initilization();

    /*
        RGB图像及相机内参订阅回调函数
        输入：ros RGB图像话题
             ros 相机参数话题
        返回：发布处理后的图像（包含目标bbx方框）
             发布目标rect话题
    */
    void imgCamCallback(const sensor_msgs::ImageConstPtr& imgMsg,
                        const sensor_msgs::CameraInfoConstPtr& infoMsg);

    /*
        RGB图像回调函数
        输入：ros RGB图像话题
        返回：发布处理后的图像（包含目标bbx方框）
             发布目标rect话题
    */
    void imgCallback(const sensor_msgs::ImageConstPtr& imgMsg);

    /*
        订阅由3D点云检测到的BBX的回调函数
        输入：
        返回：
    */
    void bbx3dCallback(const jsk_recognition_msgs::BoundingBoxConstPtr& msg);

    cv::Rect detect(cv::Mat& img,
                    cv::CascadeClassifier& cascade,
                    cv::CascadeClassifier& nestedCascade,
                    double scale );

private:

    ros::NodeHandle node_;

    image_transport::ImageTransport it_;
    image_transport::Subscriber imgSub_; // 只订阅图像
    image_transport::CameraSubscriber imgCamSub_; // 同时订阅图像及相机参数
    image_transport::Publisher imgPub_; // 发布处理后的图像

    ros::Publisher reprojected3DBBXcenterMarkerPub_; // 目标2d bbx中心投影到相机坐标系下的3d bbx中心
    ros::Publisher frustum_pub; // 锥体（由ROV 2D bbx投影生成，顶点位于相机坐标系原点），用于RVIZ显示
    ros::Publisher trapezoid3D_pub; // 梯形锥体（由ROV 2D bbx投影生成），用于RVIZ显示
    ros::Publisher rovRect_pub; // 最终检测到的ROV rect框

    ros::Subscriber cloudSub_;
    ros::Subscriber poseSub_;
    ros::Subscriber bbx3dSub_;

    image_geometry::PinholeCameraModel camModel_;

    Utility utility_;

    cv::Point2d center_;
    cv::Point2d pp1PixelFrame_, pp2PixelFrame_, pp3PixelFrame_, pp4PixelFrame_;
    cv::Point2i roiP1_, roiP2_;

    geometry_msgs::Point center3d_;

    //! 相机内参----------------------------------------------------------
    // # Intrinsic camera matrix for the raw (distorted) images.
    // #     [fx  0 cx]
    // # K = [ 0 fy cy]
    // #     [ 0  0  1]
    // # Projects 3D points in the camera coordinate frame to 2D pixel
    // # coordinates using the focal lengths (fx, fy) and principal point
    // # (cx, cy).
    Matrix3f K_;

    /*
    # Projection/camera matrix
    #     [fx'  0  cx' Tx]
    # P = [ 0  fy' cy' Ty]
    #     [ 0   0   1   0]
    # By convention, this matrix specifies the intrinsic (camera) matrix
    #  of the processed (rectified) image. That is, the left 3x3 portion
    #  is the normal camera intrinsic matrix for the rectified image.
    # It projects 3D points in the camera coordinate frame to 2D pixel
    #  coordinates using the focal lengths (fx', fy') and principal point
    #  (cx', cy') - these may differ from the values in K.
    # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
    #  also have R = the identity and P[1:3,1:3] = K.
    # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
    #  position of the optical center of the second camera in the first
    #  camera's frame. We assume Tz = 0 so both cameras are in the same
    #  stereo image plane. The first camera always has Tx = Ty = 0. For
    #  the right (second) camera of a horizontal stereo pair, Ty = 0 and
    #  Tx = -fx' * B, where B is the baseline between the cameras.
    # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
    #  the rectified image is given by:
    #  [u v w]' = P * [X Y Z 1]'
    #         x = u / w
    #         y = v / w
    #  This holds for both images of a stereo pair.
    float64[12] P # 3x4 row-major matrix
    */
    MatrixXf P_;
    //---------------------------------------------------------------

    //! 输入的原始RGB图像
    cv::Mat image_;

    //! 目标模型cascade文件路径名称
    // https://docs.opencv.org/3.4.9/d1/de5/classcv_1_1CascadeClassifier.html
    string cascadeName_;
    string nestedCascadeName_;
    cv::CascadeClassifier cascade_;
    cv::CascadeClassifier nestedCascade_;

    //! detectMultiScale()检测器函数参数 -------------------------
    //! 允许检测到的目标最大和最小尺寸（相对于图像尺寸）
    double minSizeScale_; // 整体目标检测器
    double maxSizeScale_;
    double maxColSizeScaleNested_, maxRowSizeScaleNested_; // 嵌套局部检测器
    double minColSizeScaleNested_, minRowSizeScaleNested_;
    //! 要求在同一位置连续检测到目标的数量
    int continuousDetectNum_;
    int continuousDetectNumNested_;

    //! 其他检测用参数
    double scale_; // 检测图像缩小的倍数（默认1.0），缩小检测图像可以提高检测速度，但会降低检测成功率
    bool tryflip_;
    double rectNestedScale_;
    //--------------------------------------------------------

    bool debug_;

    //! 最大有效检测景深，摄像机坐标系，单位：米
    double filterDepthValue_;

    //! 最终目标确认标志
    bool confirmedObjectFlag_;

    //! 最终目标bbx
    cv::Rect confirmedROV_;

    // 根据相机是否支持输出相机内参信息的能力来决定使用哪个图像话题订阅器
    bool imageAndCameraInfoCallback_;

    string videoName_;
    cv::VideoCapture capture;

};

}


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


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

#define _USE_MATH_DEFINES
#include "math.h"

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

INIT_TIMING
INIT_LOGGING

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

void timeStep ();
void buildModel ();
void createBodyModel();
void render ();
void reset();
void cleanup();
void initShader();
void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end);
void TW_CALL setTimeStep(const void *value, void *clientData);
void TW_CALL getTimeStep(void *value, void *clientData);
void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData);
void TW_CALL getVelocityUpdateMethod(void *value, void *clientData);
void TW_CALL setMaxIterations(const void *value, void *clientData);
void TW_CALL getMaxIterations(void *value, void *clientData);
void TW_CALL setContactTolerance(const void *value, void *clientData);
void TW_CALL getContactTolerance(void *value, void *clientData);
void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData);
void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData);
void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData);

SimulationModel model;
DistanceFieldCollisionDetection cd;
TimeStepController sim;

bool doPause = true;
std::vector<unsigned int> selectedBodies;
Vector3r oldMousePos;
Shader *shader;
Shader *shaderTex;
bool renderContacts = false;
string exePath;
string dataPath;

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

	std::string logPath = FileSystem::normalizePath(FileSystem::getProgramPath() + "/log");
	FileSystem::makeDirs(logPath);
	logger.addSink(unique_ptr<ConsoleSink>(new ConsoleSink(LogLevel::INFO)));
	logger.addSink(unique_ptr<FileSink>(new FileSink(LogLevel::DEBUG, logPath + "/PBD.log")));

	exePath = FileSystem::getProgramPath();
	dataPath = exePath + "/" + std::string(PBD_DATA_PATH);

	// OpenGL
	MiniGL::init (argc, argv, 1024, 768, 0, 0, "Rigid body collision demo");
	MiniGL::initLights ();
	MiniGL::initTexture();
	MiniGL::setClientIdleFunc (50, timeStep);
	MiniGL::setKeyFunc(0, 'r', reset);
	MiniGL::setSelectionFunc(selection);
	initShader();

	buildModel ();

	MiniGL::setClientSceneFunc(render);
	MiniGL::setViewport (40.0f, 0.1f, 500.0, Vector3r (5.0, 30.0, 70.0), Vector3r (5.0, 0.0, 0.0));

	TwAddVarRW(MiniGL::getTweakBar(), "Pause", TW_TYPE_BOOLCPP, &doPause, " label='Pause' group=Simulation key=SPACE ");
	TwAddVarRW(MiniGL::getTweakBar(), "RenderContacts", TW_TYPE_BOOLCPP, &renderContacts, " label='Render contacts' group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "TimeStepSize", TW_TYPE_REAL, setTimeStep, getTimeStep, &model, " label='Time step size'  min=0.0 max = 0.1 step=0.001 precision=4 group=Simulation ");
	TwType enumType = TwDefineEnum("VelocityUpdateMethodType", NULL, 0);
	TwAddVarCB(MiniGL::getTweakBar(), "VelocityUpdateMethod", enumType, setVelocityUpdateMethod, getVelocityUpdateMethod, &sim, " label='Velocity update method' enum='0 {First Order Update}, 1 {Second Order Update}' group=Simulation");
	TwAddVarCB(MiniGL::getTweakBar(), "MaxIter", TW_TYPE_UINT32, setMaxIterations, getMaxIterations, &sim, " label='Max. iterations'  min=1 step=1 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactTolerance", TW_TYPE_REAL, setContactTolerance, getContactTolerance, &cd, " label='Contact tolerance'  min=0.0 step=0.001 precision=3 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessRigidBody", TW_TYPE_REAL, setContactStiffnessRigidBody, getContactStiffnessRigidBody, &model, " label='Contact stiffness RB'  min=0.0 step=0.1 precision=2 group=Simulation ");
	TwAddVarCB(MiniGL::getTweakBar(), "ContactStiffnessParticleRigidBody", TW_TYPE_REAL, setContactStiffnessParticleRigidBody, getContactStiffnessParticleRigidBody, &model, " label='Contact stiffness Particle-RB'  min=0.0 step=0.1 precision=2 group=Simulation ");

	glutMainLoop ();

	cleanup ();

	Timing::printAverageTimes();

	return 0;
}

void initShader()
{
	std::string vertFile = dataPath + "/shaders/vs_smooth.glsl";
	std::string fragFile = dataPath + "/shaders/fs_smooth.glsl";
	shader = MiniGL::createShader(vertFile, "", fragFile);

	if (shader == NULL)
		return;

	shader->begin();
	shader->addUniform("modelview_matrix");
	shader->addUniform("projection_matrix");
	shader->addUniform("surface_color");
	shader->addUniform("shininess");
	shader->addUniform("specular_factor");
	shader->end();

	std::string vertFileTex = dataPath + "/shaders/vs_smoothTex.glsl";
	std::string fragFileTex = dataPath + "/shaders/fs_smoothTex.glsl";
	shaderTex = MiniGL::createShader(vertFileTex, "", fragFileTex);

	if (shaderTex == NULL)
		return;

	shaderTex->begin();
	shaderTex->addUniform("modelview_matrix");
	shaderTex->addUniform("projection_matrix");
	shaderTex->addUniform("surface_color");
	shaderTex->addUniform("shininess");
	shaderTex->addUniform("specular_factor");
	shaderTex->end();
}

void cleanup()
{
	delete TimeManager::getCurrent();
	delete shader;
	delete shaderTex;
}

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

	model.reset();
	sim.reset();
	TimeManager::getCurrent()->setTime(0.0);
}

void mouseMove(int x, int y)
{
	Vector3r mousePos;
	MiniGL::unproject(x, y, mousePos);
	const Vector3r diff = mousePos - oldMousePos;

	TimeManager *tm = TimeManager::getCurrent();
	const Real h = tm->getTimeStepSize();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	for (size_t j = 0; j < selectedBodies.size(); j++)
	{
		if (rb[selectedBodies[j]]->getMass() != 0.0)
			rb[selectedBodies[j]]->getVelocity() += 1.0 / h * diff;
	}
	oldMousePos = mousePos;
}

void selection(const Eigen::Vector2i &start, const Eigen::Vector2i &end)
{
	std::vector<unsigned int> hits;
	selectedBodies.clear();

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	std::vector<Vector3r, Eigen::aligned_allocator<Vector3r> > x;
	x.resize(rb.size());
	for (unsigned int i = 0; i < rb.size(); i++)
	{
		x[i] = rb[i]->getPosition();
	}

	Selection::selectRect(start, end, &x[0], &x[rb.size() - 1], selectedBodies);
	if (selectedBodies.size() > 0)
		MiniGL::setMouseMoveFunc(GLUT_MIDDLE_BUTTON, mouseMove);
	else
		MiniGL::setMouseMoveFunc(-1, NULL);

	MiniGL::unproject(end[0], end[1], oldMousePos);
}

void timeStep ()
{
	if (doPause)
		return;

	// Simulation code
	for (unsigned int i = 0; i < 8; i++)
		sim.step(model);
}

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

	sim.setCollisionDetection(model, &cd);

	createBodyModel();
}

void renderBallJoint(BallJoint &bj)
{
	float jointColor[4] = { 0.0, 0.6f, 0.2f, 1 };
	MiniGL::drawSphere(bj.m_jointInfo.col(2), 0.15f, jointColor);
}

void renderContact(RigidBodyContactConstraint &cc)
{
	float col1[4] = { 0.0, 0.6f, 0.2f, 1 };
	float col2[4] = { 0.6f, 0.0, 0.2f, 1 };
	MiniGL::drawPoint(cc.m_constraintInfo.col(0), 5.0, col1);
	MiniGL::drawPoint(cc.m_constraintInfo.col(1), 5.0, col2);
	MiniGL::drawVector(cc.m_constraintInfo.col(1), cc.m_constraintInfo.col(1) + cc.m_constraintInfo.col(2), 1.0, col2);
}

void render ()
{
	MiniGL::coordinateSystem();

	// Draw sim model

	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();
	SimulationModel::RigidBodyContactConstraintVector &contacts = model.getRigidBodyContactConstraints();

	float selectionColor[4] = { 0.8f, 0.0f, 0.0f, 1 };
	float surfaceColor[4] = { 0.3f, 0.5f, 0.8f, 1 };
	float staticColor[4] = { 0.5f, 0.5f, 0.5f, 1 };

	if (renderContacts)
	{
		for (unsigned int i = 0; i < contacts.size(); i++)
			renderContact(contacts[i]);
	}

	if (shader)
	{
		shader->begin();
		glUniform1f(shader->getUniform("shininess"), 5.0);
		glUniform1f(shader->getUniform("specular_factor"), 0.2f);

		GLfloat matrix[16];
		glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		glUniformMatrix4fv(shader->getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
		GLfloat pmatrix[16];
		glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
		glUniformMatrix4fv(shader->getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);
	}

	for (size_t i = 0; i < rb.size(); i++)
	{
		bool selected = false;
		for (unsigned int j = 0; j < selectedBodies.size(); j++)
		{
			if (selectedBodies[j] == i)
				selected = true;
		}

		const VertexData &vd = rb[i]->getGeometry().getVertexData();
		const IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
		if (rb[i]->getMass() != 0.0)
		{
			if (!selected)
			{
				if (shader)
					glUniform3fv(shader->getUniform("surface_color"), 1, surfaceColor);
				Visualization::drawMesh(vd, mesh, 0, surfaceColor);
			}
			else
			{
				if (shader)
					glUniform3fv(shader->getUniform("surface_color"), 1, selectionColor);
				Visualization::drawMesh(vd, mesh, 0, selectionColor);
			}
		}
	}

	if (shader)
		shader->end();

	if (shaderTex)
	{
		shaderTex->begin();
		glUniform1f(shaderTex->getUniform("shininess"), 5.0);
		glUniform1f(shaderTex->getUniform("specular_factor"), 0.2f);

		GLfloat matrix[16];
		glGetFloatv(GL_MODELVIEW_MATRIX, matrix);
		glUniformMatrix4fv(shaderTex->getUniform("modelview_matrix"), 1, GL_FALSE, matrix);
		GLfloat pmatrix[16];
		glGetFloatv(GL_PROJECTION_MATRIX, pmatrix);
		glUniformMatrix4fv(shaderTex->getUniform("projection_matrix"), 1, GL_FALSE, pmatrix);
	}

	for (size_t i = 0; i < rb.size(); i++)
	{
		bool selected = false;
		for (unsigned int j = 0; j < selectedBodies.size(); j++)
		{
			if (selectedBodies[j] == i)
				selected = true;
		}

		const VertexData &vd = rb[i]->getGeometry().getVertexData();
		const IndexedFaceMesh &mesh = rb[i]->getGeometry().getMesh();
		if (rb[i]->getMass() == 0.0)
		{
			if (!selected)
			{
				if (shaderTex)
					glUniform3fv(shaderTex->getUniform("surface_color"), 1, staticColor);
				Visualization::drawTexturedMesh(vd, mesh, 0, staticColor);
			}
			else
			{
				if (shaderTex)
					glUniform3fv(shaderTex->getUniform("surface_color"), 1, selectionColor);
				Visualization::drawTexturedMesh(vd, mesh, 0, selectionColor);
			}
		}
	}

	if (shaderTex)
		shaderTex->end();

	for (size_t i = 0; i < constraints.size(); i++)
	{
		if (constraints[i]->getTypeId() == BallJoint::TYPE_ID)
		{
			renderBallJoint(*(BallJoint*)constraints[i]);
		}
	}

	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
}

/** Create the rigid body model
*/
void createBodyModel()
{
	SimulationModel::RigidBodyVector &rb = model.getRigidBodies();
	SimulationModel::ConstraintVector &constraints = model.getConstraints();

	string fileNameBox = FileSystem::normalizePath(dataPath + "/models/cube.obj");
	IndexedFaceMesh meshBox;
	VertexData vdBox;
	OBJLoader::loadObj(fileNameBox, vdBox, meshBox);

	string fileNameCylinder = FileSystem::normalizePath(dataPath + "/models/cylinder.obj");
	IndexedFaceMesh meshCylinder;
	VertexData vdCylinder;
	OBJLoader::loadObj(fileNameCylinder, vdCylinder, meshCylinder);

	string fileNameTorus = FileSystem::normalizePath(dataPath + "/models/torus.obj");
	IndexedFaceMesh meshTorus;
	VertexData vdTorus;
	OBJLoader::loadObj(fileNameTorus, vdTorus, meshTorus);

	string fileNameCube = FileSystem::normalizePath(dataPath + "/models/cube_5.obj");
	IndexedFaceMesh meshCube;
	VertexData vdCube;
	OBJLoader::loadObj(fileNameCube, vdCube, meshCube);

	string fileNameSphere = FileSystem::normalizePath(dataPath + "/models/sphere.obj");
	IndexedFaceMesh meshSphere;
	VertexData vdSphere;
	OBJLoader::loadObj(fileNameSphere, vdSphere, meshSphere);

	const unsigned int num_piles_x = 5;
	const unsigned int num_piles_z = 5;
	const Real dx_piles = 4.0;
	const Real dz_piles = 4.0;
	const Real startx_piles = -0.5 * (Real)(num_piles_x - 1)*dx_piles;
	const Real startz_piles = -0.5 * (Real)(num_piles_z - 1)*dz_piles;
	const unsigned int num_piles = num_piles_x * num_piles_z;
	const unsigned int num_bodies_x = 3;
	const unsigned int num_bodies_y = 5;
	const unsigned int num_bodies_z = 3;
	const Real dx_bodies = 6.0;
	const Real dy_bodies = 6.0;
	const Real dz_bodies = 6.0;
	const Real startx_bodies = -0.5 * (Real)(num_bodies_x - 1)*dx_bodies;
	const Real starty_bodies = 14.0;
	const Real startz_bodies = -0.5 * (Real)(num_bodies_z - 1)*dz_bodies;
	const unsigned int num_bodies = num_bodies_x * num_bodies_y * num_bodies_z;
	rb.resize(num_piles + num_bodies + 1);
	unsigned int rbIndex = 0;

	// floor
	rb[rbIndex] = new RigidBody();
	rb[rbIndex]->initBody(1.0,
		Vector3r(0.0, -0.5, 0.0),
		Quaternionr(1.0, 0.0, 0.0, 0.0),
		vdBox, meshBox, Vector3r(100.0, 1.0, 100.0));
	rb[rbIndex]->setMass(0.0);

	const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
	const unsigned int nVert = static_cast<unsigned int>(vertices->size());

	cd.addCollisionBox(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector3r(100.0, 1.0, 100.0));
	rbIndex++;

	Real current_z = startz_piles;
	for (unsigned int i = 0; i < num_piles_z; i++)
	{
		Real current_x = startx_piles;
		for (unsigned int j = 0; j < num_piles_x; j++)
		{
			rb[rbIndex] = new RigidBody();
			rb[rbIndex]->initBody(100.0,
				Vector3r(current_x, 5.0, current_z),
				Quaternionr(1.0, 0.0, 0.0, 0.0),
				vdCylinder, meshCylinder,
				Vector3r(0.5, 10.0, 0.5));
			rb[rbIndex]->setMass(0.0);

			const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
			const unsigned int nVert = static_cast<unsigned int>(vertices->size());
			cd.addCollisionCylinder(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector2r(0.5, 10.0));
			current_x += dx_piles;
			rbIndex++;
		}
		current_z += dz_piles;
	}

	Real current_y = starty_bodies;
	unsigned int currentType = 0;
	for (unsigned int i = 0; i < num_bodies_y; i++)
	{
		Real current_x = startx_bodies;
		for (unsigned int j = 0; j < num_bodies_x; j++)
		{
			Real current_z = startz_bodies;
			for (unsigned int k = 0; k < num_bodies_z; k++)
			{
				rb[rbIndex] = new RigidBody();

				Real ax = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
				Real ay = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
				Real az = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
				Real w = static_cast <Real> (rand()) / static_cast <Real> (RAND_MAX);
				Quaternionr q(w, ax, ay, az);
				q.normalize();

				currentType = rand() % 4;
				if (currentType == 0)
				{
					rb[rbIndex]->initBody(100.0,
						Vector3r(current_x, current_y, current_z),
						q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
						vdTorus, meshTorus,
						Vector3r(2.0, 2.0, 2.0));

					const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
					const unsigned int nVert = static_cast<unsigned int>(vertices->size());
					cd.addCollisionTorus(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector2r(2.0, 1.0));
				}
				else if (currentType == 1)
				{
					rb[rbIndex]->initBody(100.0,
						Vector3r(current_x, current_y, current_z),
						q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
						vdCube, meshCube,
						Vector3r(4.0, 1.0, 1.0));

					const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
					const unsigned int nVert = static_cast<unsigned int>(vertices->size());
					cd.addCollisionBox(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector3r(4.0, 1.0, 1.0));
				}
				else if (currentType == 2)
				{
					rb[rbIndex]->initBody(100.0,
						Vector3r(current_x, current_y, current_z),
						q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
						vdSphere, meshSphere);

					const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
					const unsigned int nVert = static_cast<unsigned int>(vertices->size());
					cd.addCollisionSphere(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, 1.0);
				}
				else if (currentType == 3)
				{
					rb[rbIndex]->initBody(100.0,
						Vector3r(current_x, current_y, current_z),
						q, //Quaternionr(1.0, 0.0, 0.0, 0.0),
						vdCylinder, meshCylinder,
						Vector3r(0.75, 5.0, 0.75));

					const std::vector<Vector3r> *vertices = rb[rbIndex]->getGeometry().getVertexDataLocal().getVertices();
					const unsigned int nVert = static_cast<unsigned int>(vertices->size());
					cd.addCollisionCylinder(rbIndex, CollisionDetection::CollisionObject::RigidBodyCollisionObjectType, &(*vertices)[0], nVert, Vector2r(0.75, 5.0));
			}
				currentType = (currentType + 1) % 4;
				current_z += dz_bodies;
				rbIndex++;
			}
			current_x += dx_bodies;
		}
		current_y += dy_bodies;
	}
}

void TW_CALL setTimeStep(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	TimeManager::getCurrent()->setTimeStepSize(val);
}

void TW_CALL getTimeStep(void *value, void *clientData)
{
	*(Real *)(value) = TimeManager::getCurrent()->getTimeStepSize();
}

void TW_CALL setVelocityUpdateMethod(const void *value, void *clientData)
{
	const short val = *(const short *)(value);
	((TimeStepController*)clientData)->setVelocityUpdateMethod((unsigned int)val);
}

void TW_CALL getVelocityUpdateMethod(void *value, void *clientData)
{
	*(short *)(value) = (short)((TimeStepController*)clientData)->getVelocityUpdateMethod();
}

void TW_CALL setMaxIterations(const void *value, void *clientData)
{
	const unsigned int val = *(const unsigned int *)(value);
	((TimeStepController*)clientData)->setMaxIterations(val);
}

void TW_CALL getMaxIterations(void *value, void *clientData)
{
	*(unsigned int *)(value) = ((TimeStepController*)clientData)->getMaxIterations();
}

void TW_CALL setContactStiffnessRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessRigidBody(val);
}

void TW_CALL getContactStiffnessRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessRigidBody();
}

void TW_CALL setContactStiffnessParticleRigidBody(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((SimulationModel*)clientData)->setContactStiffnessParticleRigidBody(val);
}

void TW_CALL getContactStiffnessParticleRigidBody(void *value, void *clientData)
{
	*(Real *)(value) = ((SimulationModel*)clientData)->getContactStiffnessParticleRigidBody();
}

void TW_CALL setContactTolerance(const void *value, void *clientData)
{
	const Real val = *(const Real *)(value);
	((DistanceFieldCollisionDetection*)clientData)->setTolerance(val);
}

void TW_CALL getContactTolerance(void *value, void *clientData)
{
	*(Real *)(value) = ((DistanceFieldCollisionDetection*)clientData)->getTolerance();
}


// DecPOMDPPublisher.cpp
// Copyright 2020 Tobias Krueger
// This code is partially supplied/copied from: solve_graphsensing.cpp
// Copyright 2019 Mikko Lauri
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*ROS related Imports*/
#include "ros/ros.h"
#include <ros/console.h>
#include "dec_pomdp_msgs/Policy.h"
#include "dec_pomdp_msgs/GeneratePolicies.h"
#include "dec_pomdp_msgs/ExecutionState.h"
#include <XmlRpcValue.h>
#include <string>
/*Dec POMDP Algorithm imports*/
#include <boost/program_options.hpp>
#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include "BackwardPassParticle.hpp"
#include "GraphSensingProblem.h"
#include "JointPolicyUtilities.h"
#include "Particle.hpp"
#include "PolicyInitialization.h"

/* Global Variables */
unsigned int RNG_SEED = 1234567890;
unsigned int HORIZON = 10;
unsigned int WIDTH = 3;
int IMPROVEMENT_STEPS = 9;

ros::Publisher decPomdpPub;
// The following two functions could be used to get allowed moves and Locations from a Parameter Server
std::map<int, pgi::GraphSensing::location_t> getLocationsFromParameterServer(){
  std::map<int, pgi::GraphSensing::location_t> result;
  XmlRpc::XmlRpcValue tmp_locations;
  ros::param::get("locations", tmp_locations);
  ROS_ASSERT(tmp_locations.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for(int i = 0; i < tmp_locations.size(); i++){
    ROS_ASSERT(tmp_locations[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
    auto tmp_location = tmp_locations[i];
    pgi::GraphSensing::location_t location;
    location.x = static_cast<double>(tmp_location["x"]);
    location.y = static_cast<double>(tmp_location["y"]);
    result[i] = location;
    ROS_INFO_STREAM("Location number " << i << " is at x= " << result[i].x << " and y = " << result[i].y);
  }
  return result;
}

std::map<int, std::vector<int>> getAllowedMovesFromParameterServer(){
  std::map<int, std::vector<int>> result;
  XmlRpc::XmlRpcValue tmp_allowed_moves;
  ros::param::get("allowed_moves", tmp_allowed_moves);
  ROS_ASSERT(tmp_allowed_moves.getType() == XmlRpc::XmlRpcValue::TypeArray)
  for(int i = 0; i < tmp_allowed_moves.size(); i++){
    XmlRpc::XmlRpcValue tmp_allowed_destinations = tmp_allowed_moves[i];
    ROS_ASSERT(tmp_allowed_destinations.getType() == XmlRpc::XmlRpcValue::TypeArray);
    std::vector<int> allowed_destinations;
    for(int idx = 0; idx < tmp_allowed_destinations.size(); idx++){
      allowed_destinations.push_back(static_cast<int>(tmp_allowed_destinations[idx]));
    }
    result[i] = allowed_destinations;
  }
  return result;
}

std::vector<pgi::PolicyGraph> generatePolicies(unsigned int rng_seed,
  unsigned int horizon,
  unsigned int width,
  int improvement_steps,
  int num_particles_fwd,
  int num_rollouts,
  int num_particle_rollout,
  std::size_t blind_policy_initial_joint_action,
  pgi::PolicyInitialization policy_initialization,
  bool gaussian,
  double mx,
  double my,
  double sx,
  double sy,
  std::vector<pgi::GraphSensing::location_t> initial_positions
)
{
  ROS_INFO("Generating Policies...");
  pgi::PRNG rng(rng_seed);

  // Create the GraphSensing Dec-POMDP problem
  // Get all locations and allowed_moves from Parameter Server
  // std::map<int, pgi::GraphSensing::location_t> locations = getLocationsFromParameterServer();
  // std::map<int, std::vector<int>> moves = getAllowedMovesFromParameterServer();
  //pgi::GraphSensing::set_locations(locations);
  //pgi::GraphSensing::set_allowed_moves(moves);
  ROS_INFO_STREAM("Graphsensing locations and moves initialised");
  //pgi::GraphSensing::set_joint_action_space(locations);
  pgi::JointActionSpace jas = pgi::GraphSensing::joint_action_space;
  ROS_INFO_STREAM("Graphsensing action space set with len " << jas.num_local_spaces());
  pgi::GraphSensing::StateTransitionModel t;
  pgi::GraphSensing::RewardModel r;

  // RSS
  pgi::JointObservationSpace jos =
      pgi::GraphSensing::rss_joint_observation_space;
  pgi::GraphSensing::RSSObservationModel o;

  // Initialize localy policy graphs
  std::vector<pgi::PolicyGraph> local_policy_graphs;
  for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
    local_policy_graphs.emplace_back(pgi::fixed_width(
        horizon, width, jas.num_local_indices(agent),
        jos.num_local_indices(agent)));
  }

  // Determine the way the policies are supposed to be initialized
  if (policy_initialization == pgi::PolicyInitialization::random) {
    ROS_INFO("Random policy Initialization");
    pgi::set_random(local_policy_graphs, rng);
  } else if (policy_initialization == pgi::PolicyInitialization::greedy) {
    throw std::runtime_error(
        "Greedy open loop initialization not implemented for continuous-state "
        "problems!");
  } else if (policy_initialization == pgi::PolicyInitialization::blind) {
    ROS_INFO("Blind policy Initialization");
    pgi::set_random(local_policy_graphs,
                    rng);  // to randomize the edges in the policy
    pgi::set_blind(local_policy_graphs, blind_policy_initial_joint_action, jas);
  }
  // Create JointPolicy using the local policy graphs
  ROS_INFO("Creating Joint Policy");
  pgi::JointPolicy jp(local_policy_graphs);

  ROS_INFO("Creating Particles");
  // Create initial belief
  pgi::ParticleSet<pgi::GraphSensing::state_t> init_particles;
  if (gaussian) {
    pgi::GraphSensing::sample_initial_states_gaussian(
        init_particles.states_, num_particles_fwd,
        pgi::GraphSensing::location_t{mx, my}, sx, sy, rng, initial_positions);
    ROS_INFO("Particle Filter initialized with gaussian distributed particles");
  } else {
    // creates an initial state for two agents and certain bounds
    pgi::GraphSensing::sample_initial_states(init_particles.states_,
                                             num_particles_fwd, rng, initial_positions);
    ROS_INFO("Particle Filter initialized with random particle locations");
  }
  init_particles.nodes_ =
      std::vector<pgi::JointPolicy::joint_vertex_t>(num_particles_fwd, jp.root());
  init_particles.weights_ = std::vector<double>(
      num_particles_fwd, 1.0 / static_cast<double>(num_particles_fwd));

  // TODO Where to go with this output
  std::string output_prefix;
  ros::param::get("policy_save_path", output_prefix);
  std::ofstream fvalue(output_prefix +
                        "/policy_values.txt");
  std::ofstream ftime(output_prefix +
                     "/duration_microseconds.txt");

  // Get value of initial policy and write it to a log Document (The logging part could potentially be replaced by ROS Logging)
  // Also keep it to compare later created policies to this.<
  ROS_INFO_STREAM("Estimating value");
  double best_value = estimate_value(num_rollouts, init_particles.states_,
                                     init_particles.weights_, jp, jp.root(), t,
                                     o, r, jas, jos, rng);
  fvalue << best_value << std::endl;
  ROS_INFO_STREAM("Policy value: " << best_value);

  // Writes the initial best policy and value
  for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
    std::ofstream fs(output_prefix +
                     "/best_policy_agent" + std::to_string(agent) + ".dot");
    print(fs, jp.local_policy(agent), pgi::element_names(jas.get(agent)),
          pgi::element_names(jos.get(agent)));
  }

  std::ofstream fs(output_prefix + "/best_value.txt");
  fs << best_value << std::endl;

  // Backward pass setup
  pgi::backpass::BackPassProperties props;
  props.prob_random_history_in_heuristic_improvement = 0.05;

  ROS_INFO_STREAM("Improving the Policies over " << improvement_steps << " Improvement Steps");
  for (int i = 1; i <= improvement_steps; ++i) {
    // sample new set of particles for improvement
    ROS_INFO_STREAM("Step "<< i << ": Resampling particles");
    if (gaussian) {
      pgi::GraphSensing::sample_initial_states_gaussian(
          init_particles.states_, num_particles_fwd,
          pgi::GraphSensing::location_t{mx, my}, sx, sy, rng, initial_positions);
    } else {
      pgi::GraphSensing::sample_initial_states(init_particles.states_,
                                               num_particles_fwd, rng, initial_positions);
    }
    init_particles.nodes_ =
        std::vector<pgi::JointPolicy::joint_vertex_t>(num_particles_fwd, jp.root());
    init_particles.weights_ = std::vector<double>(
        num_particles_fwd, 1.0 / static_cast<double>(num_particles_fwd));

    std::chrono::high_resolution_clock::time_point t1 =
        std::chrono::high_resolution_clock::now();
    auto bp = pgi::backpass::improve_particle(num_rollouts, num_particle_rollout, init_particles, jp, t, o, r, jas,
                                              jos, rng, props, initial_positions);
    std::chrono::high_resolution_clock::time_point t3 =
        std::chrono::high_resolution_clock::now();
    long duration =
        std::chrono::duration_cast<std::chrono::microseconds>(t3 - t1).count();
    ftime << duration << std::endl;
    ROS_INFO_STREAM("Step "<< i << ": Improved particles in " << duration << " microseconds");

    // check the new policies value
    double improved_policy_value = estimate_value(
        num_rollouts, init_particles.states_, init_particles.weights_, bp.improved_policy,
        bp.improved_policy.root(), t, o, r, jas, jos, rng);

    fvalue << improved_policy_value << std::endl;
    ROS_INFO_STREAM("Step "<< i << ": Value of improved policy= "<< improved_policy_value);

    if (improved_policy_value > best_value) {
      best_value = improved_policy_value;
      jp = bp.improved_policy;
      ROS_INFO_STREAM("Step "<< i << ": The new joint policy is better then the old one.");
      // TODO Again how do i log this information to a File
      // Update best value and policy!
      for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
        std::ofstream fs(output_prefix +
                         "/best_policy_agent" + std::to_string(agent) + ".dot");
        print(fs, jp.local_policy(agent), pgi::element_names(jas.get(agent)),
              pgi::element_names(jos.get(agent)));
      }
      std::ofstream fs(output_prefix + "/best_value.txt");
      fs << best_value << std::endl;
    }

    std::ostringstream ss;
    ss << output_prefix << "/step" << std::setw(3)
       << std::setfill('0') << i << "_";

    for (std::size_t agent = 0; agent < jas.num_local_spaces(); ++agent) {
      std::ofstream fs(ss.str() + "agent" + std::to_string(agent) + ".dot");
      print(fs, bp.improved_policy.local_policy(agent),
            pgi::element_names(jas.get(agent)),
            pgi::element_names(jos.get(agent)));
    }
  }

  pgi::ForwardPassParticle<pgi::GraphSensing::state_t> fwd(init_particles, jp,
                                                           t, o, jas, jos, rng);
  std::cout << "Forward pass results: \n";
  for (unsigned int s = 1; s <= jp.max_steps(); ++s) {
    for (auto& q : jp.joint_vertices_with_steps_remaining(s))
    {
      auto ql = jp.to_local(q);
      for (auto& qq : ql)
        std::cout << qq << " ";

      const pgi::ParticleSet<pgi::GraphSensing::state_t>& p = fwd.particles_at(q);
      std::cout << ": " << p.weights_.size() << " particles\n";
    }
  }
  return jp.local_policies();
}

dec_pomdp_msgs::Policy policyToMsg(pgi::PolicyGraph policy, std::string robot_name){
  dec_pomdp_msgs::Policy result;
  result.robot_name = robot_name;
  pgi::vertex_t start = pgi::find_root(policy);
  ROS_INFO_STREAM("Starting node: " << start);
  result.starting_node = start;
  // Log the action space
  pgi::JointActionSpace jas = pgi::GraphSensing::joint_action_space;
  std::vector<std::string> actions = pgi::element_names(jas.get(0));
  for (auto const& action : actions){
    ROS_INFO_STREAM("Action: "<< action);
  }
  // log the joint observation space
  pgi::JointObservationSpace jos = pgi::GraphSensing::rss_joint_observation_space;
  std::vector<std::string> observations = pgi::element_names(jos.get(0));
  for (auto const& observation : observations){
    ROS_INFO_STREAM("Observation: "<< observation);
  }

  // iterate over all vertices of the policy graphs
  for (pgi::vertex_t vert : boost::make_iterator_range(boost::vertices(policy))){
    ROS_INFO_STREAM("For Vert: "<< vert << " with pose " << policy[vert] <<" the following out edges exist:");
    result.nodes.push_back(vert);
    dec_pomdp_msgs::NodeTransition transition;
    dec_pomdp_msgs::NodeAction action;
    transition.node_number = vert;
    action.node_number = vert;
    pgi::GraphSensing::location_t vert_action = pgi::GraphSensing::index_to_loc.at(policy[vert]);
    action.pose.position.x = vert_action.x;
    action.pose.position.y = vert_action.y;
    action.pose.orientation.w = 1; // This is important for some reason ... TODO
    for (pgi::edge_t out : boost::make_iterator_range(boost::out_edges(vert, policy))){
      ROS_INFO_STREAM("Vert: " << vert << " and Edge: " << out << " with Label: "<< observations[policy[out]]);
      dec_pomdp_msgs::Edge edge;
      edge.node_number = boost::target(out, policy);
      edge.measurement_interval = pgi::GraphSensing::RSSObservationModel::getIntervalForObservation(observations[policy[out]]);
      transition.edges.push_back(edge);
    }
    result.transitions.push_back(transition);
    result.actions.push_back(action);
  }

  return result;
}

bool hanleGeneratePolicies(dec_pomdp_msgs::GeneratePolicies::Request &req,
                dec_pomdp_msgs::GeneratePolicies::Response &res)
{
  std::vector<pgi::GraphSensing::location_t> initial_locations;
  for(const dec_pomdp_msgs::ExecutionState &state : req.agent_states  ){
    const pgi::GraphSensing::location_t new_location{state.pose.pose.position.x, state.pose.pose.position.y};
    initial_locations.push_back(new_location);
  }
  /*Filling up the starting locations to atleast 2 at all times */
  while(initial_locations.size() < 2){
    const pgi::GraphSensing::location_t standard_location{0.0, 0.0};
    initial_locations.push_back(standard_location);
    ROS_INFO_STREAM("Added one filler Robot with location 0.0 0.0");
  }
  /* Insert Code to generate policy and transform it to fit policy msg */
  std::vector<pgi::PolicyGraph> policyGraphs = generatePolicies(req.seed, req.horizon, req.width, req.improvement_steps, req.num_particles, req.num_rollouts, req.num_particles_rollout, 0, pgi::PolicyInitialization::random, req.gaussian, req.mx, req.my, req.sx, req.sy, initial_locations);
  /* Finish publishing policies and give response to service request */
  std::vector<dec_pomdp_msgs::Policy> result;
  for (uint i = 0; i < policyGraphs.size(); i++){
    pgi::PolicyGraph policy = policyGraphs[i];
    std::string robot_name = "TestName";
    if(i < req.agent_states.size()){
      robot_name = req.agent_states[i].robot_name;
    }
    dec_pomdp_msgs::Policy policyMsg = policyToMsg(policy, robot_name);
    result.push_back(policyMsg);
    decPomdpPub.publish(policyMsg);
    ROS_WARN_STREAM("Done: " << policyMsg.robot_name);
  }
  res.policies = result;
  return true;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "dec_pomdp_publisher");
  ros::NodeHandle n;
  decPomdpPub = n.advertise<dec_pomdp_msgs::Policy>("dec_pomdp", 10);
  ros::ServiceServer policy_service = n.advertiseService("generate_policies", hanleGeneratePolicies);
  ROS_WARN("Policy service ready to go");
  ros::spin();

  return 0;
}


#define BOOST_TEST_MODULE TEST_DETECT_OUTLIERS
#include <boost/test/unit_test.hpp>
#include "ut_details/disable_log.h"
#include "utils/algorithms.hpp"
#include "libs/sources_handling/input_data.h"
#include "apps/detect_duplicates/duplication_action.h"
#include "apps/detect_duplicates/column_stats.h"
#include <boost/range/algorithm/generate.hpp>
#include <boost/range/algorithm/unique.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>
#include <boost/range/algorithm_ext/push_front.hpp>
#include <boost/range/adaptors.hpp>
#include <iterator>
#include <vector>
#include <iostream>

bool operator == (const input_data& left, const input_data& right)
{
    return left.value == right.value;
}

bool operator < (const input_data& left, const input_data& right)
{
    return left.value < right.value;
}

bool operator != (const input_data& left, const input_data& right)
{
    return !(left == right);
}

#if 0
struct data_generator {
    using result_type = input_data;

    data_generator(unsigned int dups) :
        duplication_entries(dups)
    {
    }

    result_type operator () () const
    {
        if (index % duplication_entries != 0) {
            value += index*1.2;
        }
        return result_type{value, index++};
    }

    mutable result_type::index_type index = 0;
    mutable result_type::value_type value = 0;
    unsigned int duplication_entries = 15;

};

struct make_test_input {
    make_test_input(unsigned int size, unsigned int dup_val) :
        data(size, input_data{})
    {
        boost::generate(data, data_generator{dup_val});
    }

    using data_type = std::vector<input_data>;

    data_type data;
};

BOOST_AUTO_TEST_CASE( test_cleanup_no_action )
{
    static const auto test_data = make_test_input(42, 4);
    // we would like to generate a list without the duplications here
    make_test_input::data_type unique_entries;
    boost::copy(test_data.data | boost::adaptors::uniqued,  std::back_inserter(unique_entries));
    BOOST_TEST_REQUIRE(utils::size(test_data.data) != utils::size(unique_entries));
    std::cout<<"unique entries\n";
    boost::copy(unique_entries, std::ostream_iterator<input_data>(std::cout, ", "));
    std::cout<<"\n-----------------------------------\nall entries\n";
    boost::copy(test_data.data, std::ostream_iterator<input_data>(std::cout, ", "));
    std::cout<<std::endl;
}
#endif

using test_data = std::vector<input_data>;

test_data get_unique_data(unsigned int row_num = 1)
{
    const test_data unique_values {
        {1.12345, ++row_num}, {2.12345, ++row_num}, {3.12345, ++row_num}, {4.12345, ++row_num},
        {5.12365, ++row_num}, {6.12355, ++row_num}, {7.12344, ++row_num}, {8.12333, ++row_num},
        {15.12345, ++row_num}, {16.12345, ++row_num}, {17.12345, ++row_num}, {18.12345, ++row_num},
        {1.12344, ++row_num}, {1.12445, ++row_num}, {1.12, ++row_num}, {1.1241376, ++row_num}
    };
    return unique_values;
}

test_data no_unique_values()
{
    auto uniques = get_unique_data();
    auto unique2 = get_unique_data(uniques.size()+1);
    boost::push_front(unique2, get_unique_data(uniques.size()*2+1));
    boost::push_back(uniques, unique2);
    //std::cout<<"list of none unique values:\n";
    //boost::copy(uniques, std::ostream_iterator<input_data>(std::cout, "\n"));
    //std::cout<<std::endl;
#if 0
    const test_data data {
        uniques[0], {uniques[0].value, 20}, uniques[1], uniques[2],
        {uniques[0].value, 21}, {uniques[0].value, 22}, uniques[3], uniques[4],
        {uniques[2].value, 24}, {uniques[1].value, 25}, uniques[5], uniques[6],
        {uniques[2].value, 26}, {uniques[1].value, 27}, uniques[7], uniques[8],
        {uniques[4].value, 28}, uniques[9], {uniques[9].value, 30}, uniques[10],
        {uniques[1].value, 31}, {uniques[9].value, 32}, {uniques[1].value, 33}, {uniques[4].value, 34},
        {uniques[0].value, 35}, uniques[11], uniques[12], uniques[13], uniques[14],
        uniques[15], {uniques[15].value, 38}, {uniques[14].value, 40}
    };
#endif
    return uniques;
}

BOOST_AUTO_TEST_CASE( test_no_duplicatons )
{
    do_disable_log();

    auto const test_input = get_unique_data();
    duplication_action action{duplication_first{4}};   // the resolution value, we do not really using it here..

    auto total = 0u;
    // process all input
    std::for_each(std::begin(test_input), std::end(test_input), [&total, &action] (auto& entry) {
                ++total;
                ::apply_action(action, entry);
            }
    );
    // check
    BOOST_TEST(total == test_input.size()); // make sure we process all of them
    row_numbers dups, missing;
    finding(action, dups, missing);
    BOOST_TEST(dups.empty(), "there are no duplications in this input!!");
    BOOST_TEST(missing.empty(), "there are no missings in this input!!");

    duplication_action action_last{duplication_last{5}};
    total = 0u;
    std::for_each(std::begin(test_input), std::end(test_input), [&total, &action_last] (auto& entry) {
                ++total;
                ::apply_action(action_last, entry);
            }
    );
    // check
    BOOST_TEST(total == test_input.size()); // make sure we process all of them
    row_numbers dups_last, missing_last;
    finding(action, dups, missing);
    BOOST_TEST(dups_last.empty(), "there are no duplications in this input!!");
    BOOST_TEST(missing_last.empty(), "there are no missings in this input!!");
}

BOOST_AUTO_TEST_CASE( test_duplication_first )
{
    auto test_input = no_unique_values(); // this has duplications
    duplication_action action{duplication_first{6}};   // the resolution value, we do not really using it here..

    auto total = 0u;
    std::for_each(std::begin(test_input), std::end(test_input), [&total, &action] (auto& entry) {
                            ++total;
                            //std::cout<<"processing "<<entry<<"\n";
                            ::apply_action(action, entry);
                 }
    );
    BOOST_TEST(total == test_input.size());
    row_numbers dups, missing;
    finding(action, dups, missing);
    BOOST_TEST(dups.size() > 0, "we are expecting to have duplications here!!");
    BOOST_TEST(missing.empty(), "we should have no missing values");
    //boost::copy(dups, std::ostream_iterator<row_numbers::value_type>(std::cout, "\n"));
    // we are expecting that the duplicated rows would be after the first 1/3
    auto ref_data = get_unique_data();  // these are the rows that we don't expect to find
    for (const auto& entry : ref_data) {
        auto i = std::find_if(dups.begin(), dups.end(), [entry](auto dr) {
                return dr == entry.row;
            }
        );
        BOOST_TEST((i == dups.end()), "the entry "<<entry<<" should not be listed as duplication");
    }
    BOOST_TEST(dups.size() == ref_data.size()*2);
    test_data uniques;  // we would generate a list of unique entries, since we are droping the first
    boost::sort(test_input);
    boost::copy(test_input | boost::adaptors::uniqued,  std::back_inserter(uniques));
    BOOST_TEST_REQUIRE(utils::size(uniques) != utils::size(test_input));
    auto diff = utils::size(test_input) - utils::size(uniques);
    BOOST_TEST_REQUIRE(diff == dups.size());
}

BOOST_AUTO_TEST_CASE( test_duplication_last )
{
    auto test_input = no_unique_values(); // this has duplications
    duplication_action action{duplication_last{6}};   // the resolution value, we do not really using it here..
    auto total = 0u;
    std::for_each(std::begin(test_input), std::end(test_input), [&total, &action] (auto& entry) {
                            ++total;
                            //std::cout<<"processing "<<entry<<"\n";
                            ::apply_action(action, entry);
                 }
    );
    BOOST_TEST(total == test_input.size());
    row_numbers dups, missing;
    finding(action, dups, missing);
    BOOST_TEST(dups.size() > 0, "we are expecting to have duplications here!!");
    BOOST_TEST(missing.empty(), "we should have no missing values");
    //boost::copy(dups, std::ostream_iterator<row_numbers::value_type>(std::cout, "\n"));
    // we are expecting that the duplicated rows would be after the first 1/3
    auto ref_data = get_unique_data(17);  // these are the rows that we don't expect to find
    for (const auto& entry : ref_data) {
        auto i = std::find_if(dups.begin(), dups.end(), [entry](auto dr) {
                return dr == entry.row;
            }
        );
        BOOST_TEST((i == dups.end()), "the entry "<<entry<<" should not be listed as duplication");
    }
    BOOST_TEST(dups.size() == ref_data.size()*2);
}

BOOST_AUTO_TEST_CASE( test_duplication_resolution )
{
    auto test_input = no_unique_values(); // this has duplications
    duplication_action action{duplication_first{2}};   // the resolution value, we do not really using it here..
    auto total = 0u;
    std::for_each(std::begin(test_input), std::end(test_input), [&total, &action] (auto& entry) {
                            ++total;
                            //std::cout<<"processing "<<entry<<"\n";
                            ::apply_action(action, entry);
                 }
    );
    BOOST_TEST(total == test_input.size());
    row_numbers dups, missing;
    finding(action, dups, missing);
    BOOST_TEST(dups.size() > 0, "we are expecting to have duplications here!!");
    BOOST_TEST(missing.empty(), "we should have no missing values");
    //boost::copy(dups, std::ostream_iterator<row_numbers::value_type>(std::cout, "\n"));
    // we are expecting that the duplicated rows would be after the first 1/3
    auto ref_data = get_unique_data();  // these are the rows that we don't expect to find
    for (auto index = 0u; index < (ref_data.size() - 4u); ++index) {
        auto entry = ref_data[index];
        auto i = std::find_if(dups.begin(), dups.end(), [entry](auto dr) {
                return dr == entry.row;
            }
        );
        BOOST_TEST((i == dups.end()), "the entry "<<entry<<" should not be listed as duplication");
    }
    for (auto index = (ref_data.size() - 4u); index < ref_data.size(); ++index) {
        auto entry = ref_data[index];
        auto i = std::find_if(dups.begin(), dups.end(), [entry](auto dr) {
                    return dr == entry.row;
                }
        );
        BOOST_TEST((i != dups.end()), "we are expecting "<<entry<<" to be in duplication list");
    }
    BOOST_TEST(dups.size() == ref_data.size()*2 + 4);
}


#include "partition/multi_level_graph.hpp"
#include "util/typedefs.hpp"

#include "../common/range_tools.hpp"

#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <random>
#include <vector>

using namespace osrm;
using namespace osrm::partition;

namespace
{
struct MockEdge
{
    NodeID source;
    NodeID target;
};

auto makeGraph(const MultiLevelPartition &mlp, const std::vector<MockEdge> &mock_edges)
{
    struct EdgeData
    {
        bool forward;
        bool backward;
    };
    using Edge = util::static_graph_details::SortableEdgeWithData<EdgeData>;
    std::vector<Edge> edges;
    std::size_t max_id = 0;
    for (const auto &m : mock_edges)
    {
        max_id = std::max<std::size_t>(max_id, std::max(m.source, m.target));
        edges.push_back(Edge{m.source, m.target, true, false});
        edges.push_back(Edge{m.target, m.source, false, true});
    }
    std::sort(edges.begin(), edges.end());
    return MultiLevelGraph<EdgeData, osrm::storage::Ownership::Container>(mlp, max_id + 1, edges);
}
}

BOOST_AUTO_TEST_SUITE(multi_level_graph)

BOOST_AUTO_TEST_CASE(check_edges_sorting)
{
    // node:                0  1  2  3  4  5  6  7  8  9 10 11 12 13
    std::vector<CellID> l1{{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6}};
    std::vector<CellID> l2{{0, 0, 0, 0, 1, 1, 1, 1, 2, 2, 3, 3, 4, 4}};
    std::vector<CellID> l3{{0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2}};
    std::vector<CellID> l4{{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1}};
    MultiLevelPartition mlp{{l1, l2, l3, l4}, {7, 5, 3, 2}};

    std::vector<MockEdge> edges = {
        // edges sorted into border/internal by level
        //  level:  (1) (2) (3) (4)
        {0, 1},   //  i   i   i   i
        {2, 3},   //  i   i   i   i
        {3, 7},   //  b   b   b   i
        {4, 0},   //  b   b   b   i
        {4, 5},   //  i   i   i   i
        {5, 6},   //  b   i   i   i
        {6, 4},   //  b   i   i   i
        {6, 7},   //  i   i   i   i
        {7, 11},  //  b   b   i   i
        {8, 9},   //  i   i   i   i
        {9, 8},   //  i   i   i   i
        {10, 11}, //  i   i   i   i
        {11, 10}, //  i   i   i   i
        {11, 12}, //  b   b   b   b
        {12, 13}  //  i   i   i   i
    };

    auto graph = makeGraph(mlp, edges);

    for (auto from : util::irange(0u, graph.GetNumberOfNodes()))
    {
        LevelID level = 0;
        for (auto edge : graph.GetAdjacentEdgeRange(from))
        {
            auto to = graph.GetTarget(edge);
            BOOST_CHECK(mlp.GetHighestDifferentLevel(from, to) >= level);
            level = mlp.GetHighestDifferentLevel(from, to);
        }
    }

    // on level 0 every edge is a border edge
    for (auto node : util::irange<NodeID>(0, 13))
        CHECK_EQUAL_COLLECTIONS(graph.GetBorderEdgeRange(0, node),
                                graph.GetAdjacentEdgeRange(node));

    // on level 0 there are no internal edges
    for (auto node : util::irange<NodeID>(0, 13))
        CHECK_EQUAL_COLLECTIONS(graph.GetInternalEdgeRange(0, node), util::irange<EdgeID>(0, 0));

    // the union of border and internal edge needs to equal the adjacent edges
    for (auto level : util::irange<LevelID>(0, 4))
    {
        for (auto node : util::irange<NodeID>(0, 14))
        {
            const auto adjacent = graph.GetAdjacentEdgeRange(node);
            const auto border = graph.GetBorderEdgeRange(level, node);
            const auto internal = graph.GetInternalEdgeRange(level, node);
            std::vector<EdgeID> merged;
            std::copy(internal.begin(), internal.end(), std::back_inserter(merged));
            std::copy(border.begin(), border.end(), std::back_inserter(merged));
            CHECK_EQUAL_COLLECTIONS(adjacent, merged);
        }
    }

    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 0).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 1).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 2).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 3).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 4).size(), 2);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 5).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 6).size(), 2);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 7).size(), 2);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 8).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 9).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 10).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 11).size(), 2);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 12).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 13).size(), 0);

    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 0).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 1).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 2).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 3).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 4).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 5).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 6).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 7).size(), 2);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 8).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 9).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 10).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 11).size(), 2);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 12).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 13).size(), 0);

    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 0).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 1).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 2).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 3).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 4).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 5).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 6).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 7).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 8).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 9).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 10).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 11).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 12).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 13).size(), 0);

    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 0).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 1).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 2).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 3).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 4).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 5).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 6).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 7).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 8).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 9).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 10).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 11).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(3, 12).size(), 1);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(4, 13).size(), 0);

    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 0), graph.FindEdge(0, 4));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 3), graph.FindEdge(3, 7));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 4), graph.FindEdge(4, 6), graph.FindEdge(4, 0));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 5), graph.FindEdge(5, 6));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 6), graph.FindEdge(6, 4), graph.FindEdge(6, 5));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 7), graph.FindEdge(7, 11), graph.FindEdge(7, 3));
    CHECK_EQUAL_RANGE(
        graph.GetBorderEdgeRange(1, 11), graph.FindEdge(11, 7), graph.FindEdge(11, 12));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(1, 12), graph.FindEdge(12, 11));

    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(2, 0), graph.FindEdge(0, 4));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(2, 3), graph.FindEdge(3, 7));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(2, 4), graph.FindEdge(4, 0));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(2, 7), graph.FindEdge(7, 11), graph.FindEdge(7, 3));
    CHECK_EQUAL_RANGE(
        graph.GetBorderEdgeRange(2, 11), graph.FindEdge(11, 7), graph.FindEdge(11, 12));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(2, 12), graph.FindEdge(12, 11));

    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(3, 0), graph.FindEdge(0, 4));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(3, 3), graph.FindEdge(3, 7));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(3, 4), graph.FindEdge(4, 0));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(3, 7), graph.FindEdge(7, 3));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(3, 11), graph.FindEdge(11, 12));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(3, 12), graph.FindEdge(12, 11));

    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(4, 11), graph.FindEdge(11, 12));
    CHECK_EQUAL_RANGE(graph.GetBorderEdgeRange(4, 12), graph.FindEdge(12, 11));
}

BOOST_AUTO_TEST_CASE(check_last_internal_edge)
{
    //                      a--b--c--d
    std::vector<CellID> l1{{0, 0, 1, 1}};
    std::vector<CellID> l2{{0, 0, 1, 1}};
    MultiLevelPartition mlp{{l1, l2}, {2, 2}};

    std::vector<MockEdge> edges = {{0, 1}, {1, 0}, {1, 2}, {2, 1}, {2, 3}, {3, 2}};

    auto graph = makeGraph(mlp, edges);

    auto all_edges = graph.GetAdjacentEdgeRange(3);
    CHECK_EQUAL_COLLECTIONS(graph.GetBorderEdgeRange(0, 3), all_edges);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(1, 3).size(), 0);
    BOOST_CHECK_EQUAL(graph.GetBorderEdgeRange(2, 3).size(), 0);

    BOOST_CHECK_EQUAL(graph.GetInternalEdgeRange(0, 3).size(), 0);
    CHECK_EQUAL_COLLECTIONS(graph.GetInternalEdgeRange(1, 3), all_edges);
    CHECK_EQUAL_COLLECTIONS(graph.GetInternalEdgeRange(2, 3), all_edges);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2011-2012 by the BEM++ Authors
//
// 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 "maxwell_3d_identity_operator.hpp"

#include "boundary_operator.hpp"

#include "../common/boost_make_shared_fwd.hpp"
#include "../fiber/default_test_trial_integral_imp.hpp"
#include "../fiber/explicit_instantiation.hpp"
#include "../fiber/maxwell_3d_test_trial_integrand_functor.hpp"

#include <boost/type_traits/is_complex.hpp>

namespace Bempp
{

template <typename BasisFunctionType, typename ResultType>
Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::Maxwell3dIdentityOperator(
        const shared_ptr<const Space<BasisFunctionType> >& domain,
        const shared_ptr<const Space<BasisFunctionType> >& range,
        const shared_ptr<const Space<BasisFunctionType> >& dualToRange,
        const std::string& label,
        int symmetry) :
    Base(domain, range, dualToRange, label, symmetry)
{
    typedef Fiber::Maxwell3dTestTrialIntegrandFunctor<
            BasisFunctionType, ResultType> IntegrandFunctor;
    typedef Fiber::DefaultTestTrialIntegral<IntegrandFunctor> Integral;
    m_integral.reset(new Integral((IntegrandFunctor())));
}

template <typename BasisFunctionType, typename ResultType>
Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::~Maxwell3dIdentityOperator()
{
}

template <typename BasisFunctionType, typename ResultType>
const typename Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::CollectionOfBasisTransformations&
Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::testTransformations() const
{
    return this->dualToRange()->shapeFunctionValue();
}

template <typename BasisFunctionType, typename ResultType>
const typename Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::CollectionOfBasisTransformations&
Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::trialTransformations() const
{
    return this->domain()->shapeFunctionValue();
}

template <typename BasisFunctionType, typename ResultType>
const typename Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::TestTrialIntegral&
Maxwell3dIdentityOperator<BasisFunctionType, ResultType>::integral() const
{
    return *m_integral;
}

template <typename BasisFunctionType, typename ResultType>
BoundaryOperator<BasisFunctionType, ResultType>
maxwell3dIdentityOperator(const shared_ptr<const Context<BasisFunctionType, ResultType> >& context,
                 const shared_ptr<const Space<BasisFunctionType> >& domain,
                 const shared_ptr<const Space<BasisFunctionType> >& range,
                 const shared_ptr<const Space<BasisFunctionType> >& dualToRange,
                 const std::string& label,
                 int symmetry)
{
    typedef Maxwell3dIdentityOperator<BasisFunctionType, ResultType> Id;
    return BoundaryOperator<BasisFunctionType, ResultType>(
                context,
                boost::make_shared<Id>(domain, range, dualToRange,
                                       label, symmetry));
}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, RESULT) \
    template BoundaryOperator<BASIS, RESULT> \
    maxwell3dIdentityOperator( \
        const shared_ptr<const Context<BASIS, RESULT> >&, \
        const shared_ptr<const Space<BASIS> >&, \
        const shared_ptr<const Space<BASIS> >&, \
        const shared_ptr<const Space<BASIS> >&, \
        const std::string&, \
        int)
FIBER_ITERATE_OVER_BASIS_AND_RESULT_TYPES(INSTANTIATE_NONMEMBER_CONSTRUCTOR);

FIBER_INSTANTIATE_CLASS_TEMPLATED_ON_BASIS_AND_RESULT(Maxwell3dIdentityOperator);

} // namespace Bempp


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin *
* Department: Oden Institute and Institute for Cellular and Molecular Biology
*
* PI: Edward Marcotte *
* Project: Protein Fluorosequencing *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "make-node.h"

// Standard C++ library headers:
#include <typeinfo>
#include <vector>

// Local project headers:
#include "kd-tree/internal-node.h"
#include "kd-tree/leaf-node.h"
#include "kd-tree/node.h"

namespace whatprot {
namespace kd_tree {

namespace {
using boost::unit_test::tolerance;
using std::vector;
const double TOL = 0.000000001;
}  // namespace

// Class we can use as a template parameter for E.
class Vec {
public:
    Vec(vector<double> v) : v(v), hits(1) {}
    Vec(vector<double> v, int hits) : v(v), hits(hits) {}
    double& operator[](int d) {
        return v[d];
    }
    double operator[](int d) const {
        return v[d];
    }
    // Needed to check arguments with FakeIt.
    bool operator==(const Vec& other) const {
        return (v == other.v) && (hits == other.hits);
    }
    vector<double> v;
    int hits;
};

BOOST_AUTO_TEST_SUITE(kd_tree_suite)
BOOST_AUTO_TEST_SUITE(internal_node_suite)

BOOST_AUTO_TEST_CASE(leaf_node_test, *tolerance(TOL)) {
    int k = 2;
    int d = 3;
    vector<Vec> vecs(2, Vec(vector<double>(3, 0)));
    vecs[0][0] = 0.0;
    vecs[0][1] = 10.0;
    vecs[0][2] = 0.2;
    vecs[1][0] = 1.0;
    vecs[1][1] = 20.0;
    vecs[1][2] = 1.2;
    Node<Vec, vector<double>>* node =
            make_node<Vec, vector<double>>(k, d, &vecs[0], &vecs[2]);
    BOOST_REQUIRE(typeid(*node) == typeid(LeafNode<Vec, vector<double>>));
    LeafNode<Vec, vector<double>>* leaf_node;
    leaf_node = dynamic_cast<LeafNode<Vec, vector<double>>*>(node);
    BOOST_TEST(leaf_node->d == d);
    BOOST_TEST(leaf_node->begin == &vecs[0]);
    BOOST_TEST(leaf_node->end == &vecs[2]);
    delete node;
}

BOOST_AUTO_TEST_CASE(internal_node_test, *tolerance(TOL)) {
    int k = 2;
    int d = 3;
    vector<Vec> vecs(4, Vec(vector<double>(3, 0)));
    vecs[0][0] = 0.0;
    vecs[0][1] = 10.0;
    vecs[0][2] = 0.2;
    vecs[1][0] = 1.0;
    vecs[1][1] = 20.0;
    vecs[1][2] = 1.2;
    vecs[2][0] = 2.0;
    vecs[2][1] = 30.0;
    vecs[2][2] = 2.2;
    vecs[3][0] = 3.0;
    vecs[3][1] = 40.0;
    vecs[3][2] = 3.2;
    Node<Vec, vector<double>>* node =
            make_node<Vec, vector<double>>(k, d, &vecs[0], &vecs[4]);
    BOOST_REQUIRE(typeid(*node) == typeid(InternalNode<Vec, vector<double>>));
    InternalNode<Vec, vector<double>>* internal_node;
    internal_node = dynamic_cast<InternalNode<Vec, vector<double>>*>(node);
    BOOST_TEST(internal_node->max_left == 20.0);
    BOOST_TEST(internal_node->min_right == 30.0);
    BOOST_TEST(internal_node->split_value == 25.0);
    BOOST_TEST(internal_node->s == 1);
    BOOST_REQUIRE(typeid(*internal_node->left_child)
                  == typeid(LeafNode<Vec, vector<double>>));
    LeafNode<Vec, vector<double>>* left_child;
    left_child = dynamic_cast<LeafNode<Vec, vector<double>>*>(
            internal_node->left_child);
    BOOST_TEST(left_child->d == d);
    BOOST_TEST(left_child->begin == &vecs[0]);
    BOOST_TEST(left_child->end == &vecs[2]);
    vector<double> v0(3, 0);
    v0[0] = 0.0;
    v0[1] = 10.0;
    v0[2] = 0.2;
    BOOST_TEST(
            ((left_child->begin[0].v == v0) || (left_child->begin[1].v == v0)));
    vector<double> v1(3, 0);
    v1[0] = 1.0;
    v1[1] = 20.0;
    v1[2] = 1.2;
    BOOST_TEST(
            ((left_child->begin[0].v == v1) || (left_child->begin[1].v == v1)));
    BOOST_REQUIRE(typeid(*internal_node->right_child)
                  == typeid(LeafNode<Vec, vector<double>>));
    LeafNode<Vec, vector<double>>* right_child;
    right_child = dynamic_cast<LeafNode<Vec, vector<double>>*>(
            internal_node->right_child);
    BOOST_TEST(right_child->d == d);
    BOOST_TEST(right_child->begin == &vecs[2]);
    BOOST_TEST(right_child->end == &vecs[4]);
    vector<double> v2(3, 0);
    v2[0] = 2.0;
    v2[1] = 30.0;
    v2[2] = 2.2;
    BOOST_TEST(((right_child->begin[0].v == v2)
                || (right_child->begin[1].v == v2)));
    vector<double> v3(3, 0);
    v3[0] = 3.0;
    v3[1] = 40.0;
    v3[2] = 3.2;
    BOOST_TEST(((right_child->begin[0].v == v3)
                || (right_child->begin[1].v == v3)));
    delete node;
}

BOOST_AUTO_TEST_CASE(internal_node_reordering_test, *tolerance(TOL)) {
    int k = 2;
    int d = 3;
    vector<Vec> vecs(4, Vec(vector<double>(3, 0)));
    vecs[0][0] = 0.0;
    vecs[0][1] = 41.0;
    vecs[0][2] = 0.2;
    vecs[1][0] = 1.0;
    vecs[1][1] = 21.0;
    vecs[1][2] = 1.2;
    vecs[2][0] = 2.0;
    vecs[2][1] = 31.0;
    vecs[2][2] = 2.2;
    vecs[3][0] = 3.0;
    vecs[3][1] = 11.0;
    vecs[3][2] = 3.2;
    Node<Vec, vector<double>>* node =
            make_node<Vec, vector<double>>(k, d, &vecs[0], &vecs[4]);
    BOOST_REQUIRE(typeid(*node) == typeid(InternalNode<Vec, vector<double>>));
    InternalNode<Vec, vector<double>>* internal_node;
    internal_node = dynamic_cast<InternalNode<Vec, vector<double>>*>(node);
    BOOST_TEST(internal_node->max_left == 21.0);
    BOOST_TEST(internal_node->min_right == 31.0);
    BOOST_TEST(internal_node->split_value == 26.0);
    BOOST_TEST(internal_node->s == 1);
    BOOST_REQUIRE(typeid(*internal_node->left_child)
                  == typeid(LeafNode<Vec, vector<double>>));
    LeafNode<Vec, vector<double>>* left_child;
    left_child = dynamic_cast<LeafNode<Vec, vector<double>>*>(
            internal_node->left_child);
    BOOST_TEST(left_child->d == d);
    BOOST_TEST(left_child->begin == &vecs[0]);
    BOOST_TEST(left_child->end == &vecs[2]);
    vector<double> v0(3, 0);
    v0[0] = 3.0;
    v0[1] = 11.0;
    v0[2] = 3.2;
    BOOST_TEST(
            ((left_child->begin[0].v == v0) || (left_child->begin[1].v == v0)));
    vector<double> v1(3, 0);
    v1[0] = 1.0;
    v1[1] = 21.0;
    v1[2] = 1.2;
    BOOST_TEST(
            ((left_child->begin[0].v == v1) || (left_child->begin[1].v == v1)));
    BOOST_REQUIRE(typeid(*internal_node->right_child)
                  == typeid(LeafNode<Vec, vector<double>>));
    LeafNode<Vec, vector<double>>* right_child;
    right_child = dynamic_cast<LeafNode<Vec, vector<double>>*>(
            internal_node->right_child);
    BOOST_TEST(right_child->d == d);
    BOOST_TEST(right_child->begin == &vecs[2]);
    BOOST_TEST(right_child->end == &vecs[4]);
    vector<double> v2(3, 0);
    v2[0] = 2.0;
    v2[1] = 31.0;
    v2[2] = 2.2;
    BOOST_TEST(((right_child->begin[0].v == v2)
                || (right_child->begin[1].v == v2)));
    vector<double> v3(3, 0);
    v3[0] = 0.0;
    v3[1] = 41.0;
    v3[2] = 0.2;
    BOOST_TEST(((right_child->begin[0].v == v3)
                || (right_child->begin[1].v == v3)));
    delete node;
}

BOOST_AUTO_TEST_SUITE_END()  // internal_node_suite
BOOST_AUTO_TEST_SUITE_END()  // kd_tree_suite

}  // namespace kd_tree
}  // namespace whatprot


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file BlockMatrix.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_BLOCK_MATRIX_HPP
#define STEAM_BLOCK_MATRIX_HPP

#include <vector>
#include <map>

#include <Eigen/Core>
#include <Eigen/Sparse>

#include <steam/blockmat/BlockMatrixBase.hpp>

namespace steam {

/////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Class to store a block matrix
/////////////////////////////////////////////////////////////////////////////////////////////
class BlockMatrix : public BlockMatrixBase
{
 public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Default constructor, matrix size must still be set before using
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrix();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Rectangular matrix constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrix(const std::vector<unsigned int>& blkRowSizes,
              const std::vector<unsigned int>& blkColSizes);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Block-size-symmetric matrix constructor, pure scalar symmetry is still optional
  //////////////////////////////////////////////////////////////////////////////////////////////
  BlockMatrix(const std::vector<unsigned int>& blkSizes, bool symmetric = false);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Set entries to zero
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void zero();

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Adds the matrix to the block entry at index (r,c), block dim must match
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual void add(unsigned int r, unsigned int c, const Eigen::MatrixXd& m);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns a reference to the value at (r,c), if it exists
  ///        *Note this throws an exception if matrix is symmetric and you request a lower
  ///         triangular entry. For read operations, use copyAt(r,c).
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Eigen::MatrixXd& at(unsigned int r, unsigned int c);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Returns a copy of the entry at index (r,c)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual Eigen::MatrixXd copyAt(unsigned int r, unsigned int c) const;

 private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Matrix of block matrices (row, column)
  //////////////////////////////////////////////////////////////////////////////////////////////
  std::vector<std::vector<Eigen::MatrixXd> > data_;

};

} // steam

#endif // STEAM_BLOCK_MATRIX_HPP