#include "stdafx.h"
#include "GraphAnalysis.h"

#pragma warning ( push )
#pragma warning ( disable: 4100 )
#pragma warning ( disable: 4503 )
#pragma warning ( disable: 4510 )

#include <boost/graph/breadth_first_search.hpp>
#pragma warning ( pop )
#include "ShaderInstruction.h"
#include "SearchProjectMatrixBFSVisitor.h"

using namespace shader_analyzer;
using namespace std;
using namespace boost;

template < ShaderOperand::EComponentState state >
class IsNeededState
{
	const TGraph* pGr_;
public:
	IsNeededState( const TGraph* pGr ) : pGr_( pGr ){}
	bool operator () ( TStartVerticesArray::value_type v )
	{
		using namespace boost;
		const ShaderInstruction* pInstr = get( vertex_index, *pGr_)[v];
		if ( ( pInstr->getOperand( 0 )->getComponentState() & state ) != 0 )
			return true;
		else
			return false;
	}
};

typedef map< graph_traits< TGraph >::vertex_descriptor,
default_color_type > TColorMap;

void shader_analyzer::FindProjectionMatrixFromGraph( const TGraph& gr,
													const TStartVerticesArray& arr,
													ProjectionShaderCBs& matList )
{
	// matrix to return
	ProjectionMatrixData retData;
	// shader instruction as vertex index
	TConstVertexIndexMap vCmap = get( vertex_index, gr );
	// start vertices
	TStartVerticesArray::const_iterator Xv, Wv;

	// start vertex with x-component
	Xv = find_if( arr.begin(), arr.end(), IsNeededState< ShaderOperand::X_STATE >( &gr ) );
	if ( Xv == arr.end() )
		return;	// if there is no x-component, there is no projection matrix

	// start vertex with w-component
	Wv = find_if( arr.begin(), arr.end(), IsNeededState< ShaderOperand::W_STATE >( &gr ) );
	if ( Wv == arr.end() )
		return;	// if there is no w-component, there is no projection matrix

	// visitor to find projection matrix with breadth search algorithm
	SearchProjectMatrixBFSVisitor scbX( gr, *Xv, ShaderOperand::X_STATE );
	// find valuable operation with x-component
	breadth_first_visit( gr, *Xv, visitor( scbX ) );
	if( !scbX.getInstruction() )
		return;	// if there is no x-component valuable operation, there is no projection matrix
	const ShaderOperand* pOperand = scbX.getOperand();
	if ( pOperand == NULL )
		return;
	if ( pOperand->getOperandIndexRepresentation( D3D10_SB_OPERAND_INDEX_1D )
		!= D3D10_SB_OPERAND_INDEX_IMMEDIATE32 )
		return;	// if second index is not immediate, there is no projection matrix

	// first index of matrix constant buffer
	unsigned firstIndex = pOperand->getIndex( 0 );

	// second index of matrix constant buffer
	retData.matrixRegister = pOperand->getIndex( 1 );

	if ( Xv == Wv )	// if x- and w- components are the same vertices
	{
		switch ( scbX.getInstruction()->getOperation()->getType() )
		{
		case D3D10_SB_OPCODE_DP4:
			retData.matrixIsTransposed = true;
			retData.incorrectProjection = false;
			break;
		case D3D10_SB_OPCODE_DP3:
			retData.matrixIsTransposed = true;
			retData.incorrectProjection = true;
			break;
		case D3D10_SB_OPCODE_MUL:
		case D3D10_SB_OPCODE_MAD:
		case D3D10_SB_OPCODE_ADD:
			retData.matrixIsTransposed = false;
			retData.incorrectProjection = false;
			switch( scbX.getOperand()->getComponentState() )
			{
			case ShaderOperand::X_STATE:
				break;
			case ShaderOperand::Y_STATE:
				if (retData.matrixRegister > 0)
					retData.matrixRegister -= 1;
				break;
			case ShaderOperand::Z_STATE:
				if (retData.matrixRegister > 1)
					retData.matrixRegister -= 2;
				break;
			case ShaderOperand::W_STATE:
				if (retData.matrixRegister > 2)
					retData.matrixRegister -= 3;
				break;
			}
			break;
		default:
			return;
		}
		matList.cb[firstIndex].push_back( retData );
		matList.mask = 1 << firstIndex;
		return;
	}

	switch( scbX.getInstruction()->getOperation()->getType() )
	{
	case D3D10_SB_OPCODE_DP4:
	case D3D10_SB_OPCODE_DP3:
		retData.matrixIsTransposed = true;
		break;
	case D3D10_SB_OPCODE_MUL:
	case D3D10_SB_OPCODE_MAD:
	case D3D10_SB_OPCODE_ADD:
		retData.matrixIsTransposed = false;
		switch( scbX.getOperand()->getComponentState() )
		{
		case ShaderOperand::X_STATE:
			break;
		case ShaderOperand::Y_STATE:
			if (retData.matrixRegister > 0)
				retData.matrixRegister -= 1;
			break;
		case ShaderOperand::Z_STATE:
			if (retData.matrixRegister > 1)
				retData.matrixRegister -= 2;
			break;
		case ShaderOperand::W_STATE:
			if (retData.matrixRegister > 2)
				retData.matrixRegister -= 3;
			break;
		default:
			return;
		}
		break;
	default:
		return;
	}

	SearchProjectMatrixBFSVisitor scbW( gr, *Wv, ShaderOperand::W_STATE );
	breadth_first_visit( gr, *Wv, visitor( scbW ) );
	if( !scbW.getInstruction() )
	{
		retData.incorrectProjection = true;
	}
	else
	{
		switch( scbW.getInstruction()->getOperation()->getType() )
		{
		case D3D10_SB_OPCODE_DP4:
		case D3D10_SB_OPCODE_DP3:
		case D3D10_SB_OPCODE_MUL:
		case D3D10_SB_OPCODE_MAD:
			retData.incorrectProjection = false;
			break;
		default:
			retData.incorrectProjection = true;
			break;
		}
	}
	matList.cb[firstIndex].push_back( retData );
	matList.mask |= 1 << firstIndex;
}

unsigned FindOutputRegisterComponent( const TShaderList& shList,
									 ShaderOperand::EComponentState state,
									 unsigned outputRegisterIndex )
{
	D3D10_SB_OPERAND_TYPE type = D3D10_SB_OPERAND_TYPE_OUTPUT;
	unsigned reg = outputRegisterIndex;
	unsigned movImm = INVALID_REGISTER;
	for(int i = (int)shList.size() - 1; i > 0; i-- )
	{
#ifdef _DEBUG
		std::string strInstr = shList[i].toString();
#endif

		if( shList[i].getOperandsCount() < 2 )
			continue;
		const ShaderOperand* pBufOperand = shList[i].getOperand( 0 );
		if( pBufOperand->getOperandType() != type )
			continue;
		if( pBufOperand->getIndex( 0 ) != reg )
			continue;
		if( !( pBufOperand->getComponentState() & state) )
			continue;
		D3D10_SB_OPCODE_TYPE opType = shList[i].getOperation()->getType();
		if (opType == D3D10_SB_OPCODE_MOV)
		{
			const ShaderOperand* pSrcOperand = shList[i].getOperand( 1 );
			// for BF BC 2
			if( pSrcOperand->getOperandType() == D3D10_SB_OPERAND_TYPE_IMMEDIATE32 )
				continue;
			if( pSrcOperand->getOperandType() == D3D10_SB_OPERAND_TYPE_IMMEDIATE64 )
				continue;
			// skip move from temp register
			if( pSrcOperand->getOperandType() == D3D10_SB_OPERAND_TYPE_TEMP )
			{
				type = D3D10_SB_OPERAND_TYPE_TEMP;
				reg = pSrcOperand->getIndex(0);
				state = (ShaderOperand::EComponentState)pSrcOperand->getComponentState(state);
				movImm = (unsigned)i;
				continue;
			}
		}
		else if (opType == D3D10_SB_OPCODE_MOVC)
		{
			const ShaderOperand* pSrcOperand = shList[i].getOperand( 3 );
			// HACK: for Just Cause 2 & RE 5
			// TODO: Disable that and try search 2 matrices (operand 2 and 3)
			if( pSrcOperand->getOperandType() == D3D10_SB_OPERAND_TYPE_IMMEDIATE32 )
				continue;
			if( pSrcOperand->getOperandType() == D3D10_SB_OPERAND_TYPE_IMMEDIATE64 )
				continue;
			// skip move from temp register
			if( pSrcOperand->getOperandType() == D3D10_SB_OPERAND_TYPE_TEMP )
			{
				type = D3D10_SB_OPERAND_TYPE_TEMP;
				reg = pSrcOperand->getIndex(0);
				state = (ShaderOperand::EComponentState)pSrcOperand->getComponentState(state);
				movImm = (unsigned)i;
				continue;
			}
		}
		return (unsigned)i;
	}
	if (movImm != INVALID_REGISTER)
		return movImm;
	return INVALID_REGISTER;
}

bool GetProjectionMatrix(const TShaderList& shList,
	DWORD i, ShaderOperand::ShaderRegister &curRetInstr, ShaderOperand::ShaderRegister &curRetCB, MatrixDataWithCB<ProjectionMatrixData> &curmd );

void shader_analyzer::FindProjectionMatrixFromShader(
	const TShaderList& shList,
	unsigned outputRegisterIndex,
	ProjectionShaderCBs& matList )
{
	// TODO: If we can't find matrix, return input vector index with position
	// matrix to return
	ProjectionMatrixData retData;

	ShaderGraph shGr( shList );
	unsigned componentIndex[4];
	ShaderOperand::ShaderRegister retInstr[4];
	ShaderOperand::ShaderRegister retCB[4];
	// find component output
	for (int i = 0; i < 4; i++)
	{
		ShaderOperand::EComponentState cs = (ShaderOperand::EComponentState)(1 << i);
		componentIndex[i] = FindOutputRegisterComponent(
			shList,	cs, outputRegisterIndex );
		if( componentIndex[i] == INVALID_REGISTER )
			return;

		shGr.ClearEdgesArray();
		retInstr[i] = shGr.GetPreviousInstruction(
			componentIndex[i], cs,
			[] (const TShaderList& /*shList*/, unsigned /*startIndex*/, unsigned& /*opIndex*/) { return true; } );

		if( retInstr[i].commandIndex == INVALID_REGISTER )
		{
			retCB[i].commandIndex = INVALID_REGISTER;
			if (i == 2)
				continue;
			else
				return;
		}

#ifdef _DEBUG
		std::string strInstr = shList[retInstr[i].commandIndex].toString();
#endif

		retCB[i] = shGr.GetPreviousInstruction(
			retInstr[i].commandIndex, retInstr[i].mask,
			[] (const TShaderList& shList, unsigned startIndex, unsigned& opIndex) -> bool {
				opIndex = GetConstantBufferOperandIndex( &shList[startIndex] );
				return opIndex != 0;
		} );

		if( retCB[i].commandIndex == INVALID_REGISTER )
			return;

#ifdef _DEBUG
		std::string strCB = shList[retCB[i].commandIndex].toString();
#endif
	}

	typedef pair<bool, MatrixDataWithCB<ProjectionMatrixData>> ComponentPair;
	vector<ComponentPair> shaderData(4);
	for (int i = 0; i < 4; i++)
	{
		shaderData[i].first = GetProjectionMatrix(shList, i, retInstr[i], retCB[i], shaderData[i].second);
	}

	if (!shaderData[0].first)
		return;

	int maxFreqCount = 0;
	int maxFreqInd = 0;
	for (int i = 0; i < 4; i++)
	{
		if(shaderData[i].first)
		{
			int freqCount = 1;
			for(int j = 0; j < 4; j++)
			{
				if(shaderData[j].first)
				{
					if (shaderData[i].second.constantBuffer == shaderData[j].second.constantBuffer &&
						shaderData[i].second.md.matrixRegister == shaderData[j].second.md.matrixRegister)
						freqCount++;
				}
			}
			if(freqCount >= maxFreqCount)
			{
				maxFreqCount = freqCount;
				maxFreqInd = i;
			}
		}
	}
	if(maxFreqCount < 2)
		return;

	unsigned firstIndex = shaderData[maxFreqInd].second.constantBuffer;
	retData = shaderData[maxFreqInd].second.md;

	if( componentIndex[3] == INVALID_REGISTER )
	{
		retData.incorrectProjection = true;
	}
	else
	{
		D3D10_SB_OPCODE_TYPE WopType = shList[componentIndex[3]].getOperation()->getType();
		switch( WopType )
		{
		case D3D10_SB_OPCODE_DP4:
		case D3D10_SB_OPCODE_DP3:
		case D3D10_SB_OPCODE_MUL:
		case D3D10_SB_OPCODE_MAD:
		case D3D10_SB_OPCODE_ADD:
			retData.incorrectProjection = false;
			break;
		default:
			retData.incorrectProjection = true;
			break;
		}
	}

	matList.cb[firstIndex].push_back( retData );
	matList.mask = 1 << firstIndex;
}

bool GetProjectionMatrix( const TShaderList& shList,
	DWORD i, ShaderOperand::ShaderRegister &curRetInstr, ShaderOperand::ShaderRegister &curRetCB, MatrixDataWithCB<ProjectionMatrixData> &curmd )
{
	if( curRetInstr.commandIndex == INVALID_REGISTER )
		return false;

	auto& retInstr = shList[curRetInstr.commandIndex];
#ifdef _DEBUG
	std::string strInstr = retInstr.toString();
#endif
	D3D10_SB_OPCODE_TYPE opType = retInstr.getOperation()->getType();

	auto& retCBInstr = shList[curRetCB.commandIndex];
#ifdef _DEBUG
	std::string strCB = retCBInstr.toString();
#endif
	const ShaderOperand* op = retCBInstr.getOperand( curRetCB.operandIndex );
	curmd.constantBuffer = op->getIndex( 0 );
	curmd.md.matrixRegister = op->getIndex( 1 );

	switch( opType )
	{
	case D3D10_SB_OPCODE_DP4:
	case D3D10_SB_OPCODE_DP3:
		curmd.md.matrixIsTransposed = true;
		if (curmd.md.matrixRegister >= i)
		{
			curmd.md.matrixRegister -= i;
			return true;
		}
		return false;
	case D3D10_SB_OPCODE_MUL:
	case D3D10_SB_OPCODE_MAD:
	case D3D10_SB_OPCODE_ADD:
		curmd.md.matrixIsTransposed = false;
		ShaderOperand::EComponentState cs = (ShaderOperand::EComponentState)op->getComponentState();
		if( cs == ShaderOperand::ALL_COMPONENTS )
		{
			if (curmd.md.matrixRegister >= 3)
			{
				curmd.md.matrixRegister -= 3;
			}
			return true;
		}
		break;
	}
	return false;
}

void shader_analyzer::GetUsedResourcesMask( const TShaderList& shList, std::bitset<128> &mask )
{
	mask.reset();
	for(size_t i = 0; i < shList.size(); i++ )
	{
		auto& instruction = shList[i];
		auto operation = instruction.getOperation();
		if( !operation->isDeclaration() )
			break;

#ifdef _DEBUG
		std::string strInstr = instruction.toString();
#endif

		const UINT* pData = instruction.GetData().data();
		if ( operation->getType() ==  D3D10_SB_OPCODE_DCL_RESOURCE ||
			operation->getType() ==  D3D11_SB_OPCODE_DCL_RESOURCE_RAW ||
			operation->getType() ==  D3D11_SB_OPCODE_DCL_RESOURCE_STRUCTURED )
		{
			shader_analyzer::ShaderOperand op( const_cast<UINT*>(&pData[1]));
			if (op.getOperandIndexDimension() == 1)
			{
				if (op.getOperandIndexRepresentation(D3D10_SB_OPERAND_INDEX_0D) == D3D10_SB_OPERAND_INDEX_IMMEDIATE32)
				{
					DWORD tex = op.getIndex(0);
					mask.set(tex);
				}
			}
		}
	}
}


#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/make_shared.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <algorithm>
#include <fstream>
#include <cmath>

#include "kinect_view.h"

namespace kinect_merge {

// This is used for deciding whether two points are similar enough to be merged.
static const float SIMILARITY_THRESHOLD = 3.0f; // in units of standard deviations

// Measurement variance constants
static const float ALPHA_0 = 0.0032225f;
static const float ALPHA_1 = -0.0020925f;
static const float ALPHA_2 = 0.0022078f;
static const float BETA_X = 0.0017228f;
static const float BETA_Y = 0.0017092f;

CPoint::CPoint() {}

CPoint::CPoint(const cv::Matx31f& position, const cv::Matx33f& covariance, const cv::Matx<unsigned char, 3, 1>& color) :
    pos(position),
    cov(covariance),
    col_sum(color),
    num_merged(1) {}

cv::Matx<unsigned char, 3, 1> CPoint::get_color() const {
    // NOTE: The following line would segfault on cv::Mat(col_sum). An alternate method follows.
    // return cv::Mat(col_sum) / num_merged;
    cv::Matx<unsigned char, 3, 1> m;
    for(int i = 0; i < 3; i++)
        m(i) = col_sum(i) / num_merged;
    return m;
}

CView::CView(const std::string &extrinsic_file,
             const std::string &disparity_file,
             const std::string &color_file,
             float covariance_scale_xy,
             float covariance_scale_z,
             CKinectCalibration &calibration_) :
    calibration(calibration_),
    original_pointmap(boost::extents[IMAGE_HEIGHT][IMAGE_WIDTH]),
    pointmap(boost::extents[IMAGE_HEIGHT][IMAGE_WIDTH]),
    num_missing(0),
#ifdef DEBUG // The measurement_used vector is used for outputting debug images.
    num_added(0),
    measurement_used(IMAGE_HEIGHT) {
#else
    num_added(0) {
#endif

    // Initialize the depth map.
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            depthmap(v, u) = -1;
        }
    }

    // Load extrinsic data.

    cv::Mat inv_transformation_tmp = cv::Mat_<float>::eye(4, 4);
    cv::Mat inv_rotation(inv_transformation_tmp, cv::Rect(0, 0, 3, 3));
    cv::Mat inv_translation(inv_transformation_tmp, cv::Rect(3, 0, 1, 3));

    try {
        cv::FileStorage fs(extrinsic_file, cv::FileStorage::READ);
        fs["R"] >> inv_rotation;
        fs["T"] >> inv_translation;
    } catch(cv::Exception &e) {
        std::cerr << std::endl << "Error reading extrinsic data: " << e.what() << std::endl;
        exit(4);
    }

    inv_transformation = inv_transformation_tmp;
    cv::Mat transformation = inv_transformation_tmp.inv();
    cv::Mat rotation = inv_rotation.t();

    // Load disparity map.
    int flags = -1; // Load as-is (greyscale). Using 0 (force grayscale) results in a segfault.
    cv::Mat1s disparitymap = cv::imread(disparity_file, flags);

    if(disparitymap.cols != IMAGE_WIDTH || disparitymap.rows != IMAGE_HEIGHT) {
        std::cerr << std::endl << "Disparity map has dimensions " << disparitymap.cols << "x" << disparitymap.rows <<
                                  " instead of " << IMAGE_WIDTH << "x" << IMAGE_HEIGHT << std::endl;
        exit(5);
    }

    // Load color map.
    cv::Mat3b colormap = cv::imread(color_file, flags);

    if(colormap.cols != IMAGE_WIDTH || colormap.rows != IMAGE_HEIGHT) {
        std::cerr << std::endl << "Color map has dimensions " << colormap.cols << "x" << colormap.rows <<
                                  " instead of " << IMAGE_WIDTH << "x" << IMAGE_HEIGHT << std::endl;
        exit(6);
    }

    // Convert to a point cloud in global coordinates and calculate the covariance matrix.
    cv::Matx31f local_point;
    cv::Mat local_point_tmp = (cv::Mat_<float>(4, 1) << 0, 0, 0, 1);
    cv::Mat global_point;
    cv::Mat covariance;
    for(int v = 0; v < disparitymap.rows; v++) {
        for(int u = 0; u < disparitymap.cols; u++) {
            const short disparity = disparitymap(v, u);
            if(disparity == 2047) {
                continue;
            }

            // Convert to local and then global coordinates.
            calibration.disparity2point(u, v, disparity, local_point);
            local_point_tmp.at<float>(0) = local_point(0);
            local_point_tmp.at<float>(1) = local_point(1);
            local_point_tmp.at<float>(2) = local_point(2);
            global_point = transformation * local_point_tmp;
            global_point.pop_back(); // Turn into a 3 by 1 matrix.

            float depth = local_point(2);
            if(depth < 0) {
                // The point projects behind the camera. Ignore it.
                continue;
            }

            // Calculate the covariance matrix.
            covariance = cv::Mat_<float>::eye(3, 3);
            covariance.at<float>(0, 0) = BETA_X * BETA_X * depth * depth / 12;
            covariance.at<float>(1, 1) = BETA_Y * BETA_Y * depth * depth / 12;
            covariance.at<float>(2, 2) = pow(ALPHA_2 * depth * depth + ALPHA_1 * depth + ALPHA_0, 2);
            covariance.at<float>(0, 0) *= covariance_scale_xy;
            covariance.at<float>(1, 1) *= covariance_scale_xy;
            covariance.at<float>(2, 2) *= covariance_scale_z;

            // Convert the covariance matrix to the global reference frame.
            covariance = rotation * covariance * inv_rotation;
            assert(covariance.at<float>(0, 0) >= 0);
            assert(covariance.at<float>(1, 1) >= 0);
            assert(covariance.at<float>(2, 2) >= 0);

            // Get the pixel coordinates for the measurement.
            int uc, vc;
            to_image_plane(local_point_tmp, uc, vc);
            if(uc < 0 || uc >= IMAGE_WIDTH || vc < 0 || vc >= IMAGE_HEIGHT) {
                continue;
            }

            if(!original_pointmap[vc][uc] || depthmap(vc, uc) > depth) {
                depthmap(vc, uc) = depth;
                original_pointmap[vc][uc] = boost::make_shared<CPoint>(global_point,
                                                                       covariance,
                                                                       colormap(vc, uc));
            }
        }
    }
}

void CView::to_image_plane(const cv::Matx41f &local_pos, int &uc, int &vc) const {
    cv::Matx31f local_pos_tmp(local_pos(0), local_pos(1), local_pos(2));
    cv::Matx21f cf;
    calibration.point2rgb(local_pos_tmp, cf);
    uc = cf(0) + 0.5;
    vc = cf(1) + 0.5;
}

static float calculate_mahalanobis_distance(const CPoint &refined_point, const CPoint &p) {
    cv::Matx<float, 1, 1> operand = (p.pos - refined_point.pos).t() * p.cov.inv() * (p.pos - refined_point.pos);
    return sqrt(operand(0));
}

// Calculate a new position estimate, covariance matrix and color for an existing point given a new measurement.
static void refine_point(const CPoint &existing_point, const CPoint &new_measurement, CPoint &refined_point) {
    cv::Matx33f new_measurement_cov_inv = new_measurement.cov.inv();

    // Combine the covariance matrices.
    refined_point.cov = (existing_point.cov.inv() + new_measurement_cov_inv).inv();

    // Calculate the refined position estimate.
    refined_point.pos = existing_point.pos + refined_point.cov * new_measurement_cov_inv * (new_measurement.pos - existing_point.pos);

    // Add the color values.
    refined_point.col_sum = existing_point.col_sum + new_measurement.col_sum;
    refined_point.num_merged = existing_point.num_merged + new_measurement.num_merged;
}

void CView::refine_points(CView &connected_view, std::vector<std::vector<bool> > &measurement_used) const {
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            if(!connected_view.has_point(u, v)) {
                continue;
            }

            // Project the point onto the current view image plane.
            // NOTE: Only points which were added to cloud from the connected view are projected.
            // Each point in the cloud is pointed to by exactly one view. This means that a single
            // point in the cloud will NOT be refined multiple times with the same new measurement
            // through different connected views.
            int proj_u, proj_v;
            float proj_depth;
            CPoint &connected_view_point = connected_view.get_point(u, v);
            project(connected_view_point, proj_u, proj_v, proj_depth);

            if(proj_depth < 0) {
                // The point projects outside the image plane.
                continue;
            }

            if(!has_measurement(proj_u, proj_v)) {
                // There is nothing to merge with at this pixel.
                continue;
            }

            const CPoint &current_view_point = get_original_point(proj_u, proj_v);

            // Calculate the refined point and use it if it's close enough to the existing point and
            // new measurement.
            CPoint refined_point;
            refine_point(connected_view_point, current_view_point, refined_point);
            if(calculate_mahalanobis_distance(refined_point, connected_view_point) < SIMILARITY_THRESHOLD &&
               calculate_mahalanobis_distance(refined_point, current_view_point) < SIMILARITY_THRESHOLD) {
                connected_view_point = refined_point;
                measurement_used[proj_v][proj_u] = true;
            }
        }
    }
}

void CView::insert_into_point_cloud(std::vector<CPoint::const_ptr> &point_cloud) {
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            if(has_measurement(u, v)) {
                point_cloud.push_back(original_pointmap[v][u]);
            }
        }
    }
}

void CView::project(const CPoint &global_point, int &proj_u, int &proj_v, float &proj_depth) const {
    cv::Matx41f global_pos(global_point.pos(0),
                           global_point.pos(1),
                           global_point.pos(2),
                           1);
    cv::Matx41f local_pos = inv_transformation * global_pos;
    to_image_plane(local_pos, proj_u, proj_v);

    if(proj_u < 0 || proj_u >= IMAGE_WIDTH ||
       proj_v < 0 || proj_v >= IMAGE_HEIGHT) {
        // The point projects outside the image plane.
        proj_depth = -1;
    } else {
        proj_depth = local_pos(2);
    }
}

void CView::merge(boost::ptr_vector<CView> &views,
                  unsigned int view_idx,
                  const cv::Mat view_connectivity,
                  std::vector<CPoint::ptr> &global_point_cloud) {
    // Initialize vectors.
    // A measurement is marked as used if an existing point is refined with it.
#ifndef DEBUG
    // Use a local variable instead of a member variable.
    std::vector<std::vector<bool> > measurement_used(IMAGE_HEIGHT);
#endif
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        measurement_used[v].resize(IMAGE_WIDTH);
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            measurement_used[v][u] = false;
        }
    }

    // Project all connected views into the current view to find points to be refined.
    for(unsigned int connected_idx = 0; connected_idx < view_idx; connected_idx++) {
        if(!view_connectivity.at<bool>(view_idx, connected_idx)) {
            // The view is not connected. Skip it.
            continue;
        }

        CView &connected_view = views[connected_idx];

        refine_points(connected_view, measurement_used);
    }

    // Add all the points that weren't used to refine existing points to the point cloud.
    for(int v = 0; v < IMAGE_HEIGHT; v++) {
        for(int u = 0; u < IMAGE_WIDTH; u++) {
            if(has_measurement(u, v)) {
                if(!measurement_used[v][u]) {
                    pointmap[v][u] = original_pointmap[v][u];
                    global_point_cloud.push_back(pointmap[v][u]);
                    num_added++;
                }
            } else {
                num_missing++;
            }
        }
    }
}

}


#pragma once

//#include <mmsystem.h>
//#pragma comment(lib, "winmm.lib")

#include <xul/util/time_counter.hpp>
#include <boost/asio/basic_deadline_timer.hpp>
#include <winbase.h>

namespace xul { namespace detail {

class tick_count_timer_traits
{
public:
    class time_traits_type
    {
    public:
        // The duration type.
        class duration_type
        {
        public:
            duration_type() : ticks_(0) {}
            explicit duration_type(DWORD ticks) : ticks_(ticks) {}
        private:
            friend class time_traits_type;
            DWORD ticks_;
        };

        // The time type. This type has no constructor that takes a DWORD to ensure
        // that the timer can only be used with relative times.
        class time_type
        {
        public:
            time_type() : ticks_(0) {}
        private:
            friend class time_traits_type;
            DWORD ticks_;
        };

        // Get the current time.
        static time_type now()
        {
            time_type result;
            result.ticks_ = ::GetTickCount();
            return result;
        }

        // Add a duration to a time.
        static time_type add(const time_type& t, const duration_type& d)
        {
            time_type result;
            result.ticks_ = t.ticks_ + d.ticks_;
            return result;
        }

        // Subtract one time from another.
        static duration_type subtract(const time_type& t1, const time_type& t2)
        {
            return duration_type(t1.ticks_ - t2.ticks_);
        }

        // Test whether one time is less than another.
        static bool less_than(const time_type& t1, const time_type& t2)
        {
            // DWORD tick count values wrap periodically, so we'll use a heuristic that
            // says that if subtracting t1 from t2 yields a value smaller than 2^31,
            // then t1 is probably less than t2. This means that we can't handle
            // durations larger than 2^31, which shouldn't be a problem in practice.
            return (t2.ticks_ - t1.ticks_) < static_cast<DWORD>(1 << 31);
        }

        // Convert to POSIX duration type.
        static boost::posix_time::time_duration to_posix_duration(
            const duration_type& d)
        {
            return boost::posix_time::milliseconds(d.ticks_);
        }
    };

    typedef time_traits_type::duration_type duration_type;
    typedef time_traits_type::time_type time_type;
};

class timer_traits
{
public:
    class time_traits_type
    {
    public:
        // The duration type.
        class duration_type
        {
        public:
            duration_type() : ticks_(0) {}
            explicit duration_type(int64_t ticks) : ticks_(ticks) {}
        private:
            friend class time_traits_type;
            int64_t ticks_;
        };

        // The time type. This type has no constructor that takes a DWORD to ensure
        // that the timer can only be used with relative times.
        class time_type
        {
        public:
            time_type() : ticks_(0) {}
        private:
            friend class time_traits_type;
            int64_t ticks_;
        };

        // Get the current time.
        static time_type now()
        {
            time_type result;
            result.ticks_ = detail::accurate_tick_counter::get_current_count();
            return result;
        }

        // Add a duration to a time.
        static time_type add(const time_type& t, const duration_type& d)
        {
            time_type result;
            result.ticks_ = t.ticks_ + d.ticks_;
            return result;
        }

        // Subtract one time from another.
        static duration_type subtract(const time_type& t1, const time_type& t2)
        {
            return duration_type(t1.ticks_ - t2.ticks_);
        }

        // Test whether one time is less than another.
        static bool less_than(const time_type& t1, const time_type& t2)
        {
            return t1.ticks_ < t2.ticks_;
        }

        // Convert to POSIX duration type.
        static boost::posix_time::time_duration to_posix_duration(
            const duration_type& d)
        {
            return boost::posix_time::milliseconds(d.ticks_);
        }
    };

    typedef time_traits_type::duration_type duration_type;
    typedef time_traits_type::time_type time_type;
};

} }


/*
	Satellite Attitude Dynamics Stepper

	@author		:	siddharth deore
	@licence	:	MIT
*/

#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
// reading a text file
#include <iostream>
#include <fstream>
#include <string>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>
#include "Satellite.h"
#include <ctime>
using namespace std;
using namespace boost::numeric::odeint;

int main(int argc, char** argv)
{
	system("cls");
	// Initial condition
	double IC[7] = { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0 };
	// Read Configration file for initial condiion Each line is state, TODO:: some kind of parser!
	string line;
	ifstream myfile("conf.txt");
	int i = 0;
	if (myfile.is_open())
	{
		while (getline(myfile, line))
		{
			IC[i++] = std::stod(line);
			//cout << IC[i-1] <<std::endl;
		}
		myfile.close();
	}

	else cout << "Unable to open file";
	// Clean Results file

	std::ofstream outfile;

	outfile.open("results.csv"); // overwrite
	outfile << "time, q0, q1, q2, q3, wx, wy, wz" << endl;
	outfile.close();

	Satellite sat;
	Satellite::state_type x;
	time_t t1, t2;
	//double q0, q1, q2, q3, w0, w1, w2, w3;
	sat.setInnertia(1.0, 2.0, 3.0);
	sat.setControllerGains(0.005, 0.01); // (Kp,Kd)
	sat.setTargetQuaternion(0.0, 1.0, 0.0, 0.0); //(q0,q1,q2,q3)
	sat.setState(IC[0], IC[1], IC[2], IC[3], IC[4], IC[5], IC[6]);
	time(&t1);
	sat.step(10000.0, 0.01, x);
	time(&t2);
	std::cout << "Execution time " << t2 - t1 << std::endl;
}


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

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_MULTI_ALGORITHMS_INTERSECTION_HPP
#define BOOST_GEOMETRY_MULTI_ALGORITHMS_INTERSECTION_HPP

#include <boost/geometry/multi/core/closure.hpp>
#include <boost/geometry/multi/core/geometry_id.hpp>
#include <boost/geometry/multi/core/is_areal.hpp>
#include <boost/geometry/multi/core/point_order.hpp>
#include <boost/geometry/multi/algorithms/envelope.hpp>
#include <boost/geometry/multi/algorithms/num_points.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/get_ring.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/get_turns.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/copy_segments.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/copy_segment_point.hpp>
#include <boost/geometry/multi/algorithms/detail/overlay/select_rings.hpp>
#include <boost/geometry/multi/algorithms/detail/sections/range_by_section.hpp>
#include <boost/geometry/multi/algorithms/detail/sections/sectionalize.hpp>

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

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template
<
    typename MultiLinestring1, typename MultiLinestring2,
    typename OutputIterator, typename PointOut,
    typename Strategy
>
struct intersection_multi_linestring_multi_linestring_point
{
    static inline OutputIterator apply(MultiLinestring1 const& ml1,
            MultiLinestring2 const& ml2, OutputIterator out,
            Strategy const& strategy)
    {
        // Note, this loop is quadratic w.r.t. number of linestrings per input.
        // Future Enhancement: first do the sections of each, then intersect.
        for (typename boost::range_iterator
                <
                    MultiLinestring1 const
                >::type it1 = boost::begin(ml1);
            it1 != boost::end(ml1);
            ++it1)
        {
            for (typename boost::range_iterator
                    <
                        MultiLinestring2 const
                    >::type it2 = boost::begin(ml2);
                it2 != boost::end(ml2);
                ++it2)
            {
                out = intersection_linestring_linestring_point
                    <
                        typename boost::range_value<MultiLinestring1>::type,
                        typename boost::range_value<MultiLinestring2>::type,
                        OutputIterator, PointOut, Strategy
                    >::apply(*it1, *it2, out, strategy);
            }
        }

        return out;
    }
};

template
<
    typename Linestring, typename MultiLinestring,
    typename OutputIterator, typename PointOut,
    typename Strategy
>
struct intersection_linestring_multi_linestring_point
{
    static inline OutputIterator apply(Linestring const& linestring,
            MultiLinestring const& ml, OutputIterator out,
            Strategy const& strategy)
    {
        for (typename boost::range_iterator
                <
                    MultiLinestring const
                >::type it = boost::begin(ml);
            it != boost::end(ml);
            ++it)
        {
            out = intersection_linestring_linestring_point
                <
                    Linestring,
                    typename boost::range_value<MultiLinestring>::type,
                    OutputIterator, PointOut, Strategy
                >::apply(linestring, *it, out, strategy);
        }

        return out;
    }
};

template
<
    typename MultiLinestring, typename Box,
    typename OutputIterator, typename LinestringOut,
    typename Strategy
>
struct clip_multi_linestring
{
    static inline OutputIterator apply(MultiLinestring const& multi_linestring,
            Box const& box, OutputIterator out, Strategy const& strategy)
    {
        typedef typename point_type<LinestringOut>::type point_type;
        strategy::intersection::liang_barsky<Box, point_type> lb_strategy;
        for (typename boost::range_iterator<MultiLinestring const>::type it
            = boost::begin(multi_linestring);
            it != boost::end(multi_linestring); ++it)
        {
            out = detail::intersection::clip_range_with_box
                <LinestringOut>(box, *it, out, lb_strategy);
        }
        return out;
    }
};

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// Linear
template
<
    typename MultiLinestring1, typename MultiLinestring2,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator, typename GeometryOut,
    overlay_type OverlayType,
    typename Strategy
>
struct intersection_insert
    <
        multi_linestring_tag, multi_linestring_tag, point_tag,
        false, false, false,
        MultiLinestring1, MultiLinestring2,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        OverlayType,
        Strategy
    > : detail::intersection::intersection_multi_linestring_multi_linestring_point
            <
                MultiLinestring1, MultiLinestring2,
                OutputIterator, GeometryOut,
                Strategy
            >
{};

template
<
    typename Linestring, typename MultiLinestring,
    typename OutputIterator, typename GeometryOut,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    overlay_type OverlayType,
    typename Strategy
>
struct intersection_insert
    <
        linestring_tag, multi_linestring_tag, point_tag,
        false, false, false,
        Linestring, MultiLinestring,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        OverlayType,
        Strategy
    > : detail::intersection::intersection_linestring_multi_linestring_point
            <
                Linestring, MultiLinestring,
                OutputIterator, GeometryOut,
                Strategy
            >
{};

template
<
    typename MultiLinestring, typename Box,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename OutputIterator, typename GeometryOut,
    overlay_type OverlayType,
    typename Strategy
>
struct intersection_insert
    <
        multi_linestring_tag, box_tag, linestring_tag,
        false, true, false,
        MultiLinestring, Box,
        Reverse1, Reverse2, ReverseOut,
        OutputIterator, GeometryOut,
        OverlayType,
        Strategy
    > : detail::intersection::clip_multi_linestring
            <
                MultiLinestring, Box,
                OutputIterator, GeometryOut,
                Strategy
            >
{};

} // namespace dispatch
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_MULTI_ALGORITHMS_INTERSECTION_HPP


/*=============================================================================
    Copyright (c) 2011 Eric Niebler

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

#if !defined(BOOST_FUSION_SINGLE_VIEW_DISTANCE_IMPL_JUL_07_2011_1348PM)
#define BOOST_FUSION_SINGLE_VIEW_DISTANCE_IMPL_JUL_07_2011_1348PM

#include <boost/fusion/support/config.hpp>
#include <boost/mpl/minus.hpp>

namespace boost { namespace fusion
{
    struct single_view_iterator_tag;

    namespace extension
    {
        template<typename Tag>
        struct distance_impl;

        template<>
        struct distance_impl<single_view_iterator_tag>
        {
            template<typename First, typename Last>
            struct apply
              : mpl::minus<typename Last::position, typename First::position>
            {
                typedef typename mpl::minus<typename Last::position,
                    typename First::position>::type type;

                BOOST_CONSTEXPR BOOST_FUSION_GPU_ENABLED
                static type
                call(First const& /*first*/, Last const& /*last*/)
                {
                    return type();
                }
            };
        };
    }

}}

#endif


// Copyright (c) Microsoft Corporation. All rights reserved.
// Copyright (c) 2020 Syed Jafri. All rights reserved.
// Licensed under the MIT License

#pragma once

#include "tables.hpp"
#include "context.hpp"
#include "stack.hpp"
#include <boost/multiprecision/cpp_int.hpp>

namespace wasm_evm {
  using BN = boost::multiprecision::number<boost::multiprecision::cpp_int_backend<>>;

  // Forward Declarations
  class evm;

  class Processor {
  private:
    EthereumTransaction& transaction;           // the transaction object
    evm* contract;                              // pointer to parent contract
    Context* ctx;                               // pointer to the current context
    std::vector<std::unique_ptr<Context>> ctxs; // the stack of contexts (one per nested call)

  public:

    Processor(EthereumTransaction& transaction, evm* contract)
      : transaction(transaction),
        contract(contract)
    {}

    void process_transaction(const Account& caller);
    ExecResult initialize_create(const Account& caller);
    ExecResult initialize_call(const Account& caller);
    void run();

    uint16_t get_call_depth() const;
    const uint8_t get_op() const;
    void revert_state(const size_t& revert_to);
    void pop_context();
    void refund_gas(uint256_t amount);
    void push_context(
      const size_t sm_checkpoint,
      const Account& caller,
      const Account& callee,
      uint256_t gas_left,
      const bool is_static,
      const uint256_t call_value,
      std::vector<uint8_t>&& input,
      Program&& prog,
      SuccessHandler&& success_cb,
      ErrorHandler&& error_cb
    );
    void dispatch();

    // Can return/throw errors
    bool access_mem(const uint256_t& offset, const uint256_t& size);
    bool prepare_mem_access(const uint256_t& offset, const uint64_t& size);
    bool jump_to(const uint256_t& newPc);
    bool use_gas(uint256_t amount);
    bool process_sstore_gas(uint256_t original_value, uint256_t current_value, uint256_t new_value);
    bool throw_error(const Exception& exception, const std::vector<uint8_t>& output);

    // State
    struct AccountResult {
      const Account& account;
      const bool error;
    };
    const Account& get_account(const Address& address);
    AccountResult create_account(
      const Address& address,
      const bool& is_contract = false
    );
    void increment_nonce(const Address& address);
    void set_code(const Address& address, const std::vector<uint8_t>& code);
    void selfdestruct(const Address& addr);
    void kill_storage(const Address& address);
    bool transfer_internal(const Address& from, const Address& to, const uint256_t& amount);
    void throw_stack();

    // Reverting
    void remove_code(const Address& address);
    void remove_account(const Address& address);
    void decrement_nonce(const Address& address);

    // Storage
    void storekv(const Address& address, const uint256_t& key, const uint256_t& value);
    uint256_t loadkv(const Address& address, const uint256_t& key);

    // Precompile
    void precompile_execute(uint256_t address);
    void precompile_return(const std::vector<uint8_t>& output);
    void precompile_not_implemented();
    void precompile_ecrecover();

    void precompile_sha256();
    void precompile_ripemd160();
    void precompile_identity();
    void precompile_blake2b();

    // Exponential mod
    void precompile_expmod();
    uint256_t adjusted_exponent_length(uint256_t exponent_length, uint256_t base_length);
    uint256_t mult_complexity(const uint256_t& len);
    uint256_t read_input(uint64_t offset, uint64_t length);

    // Boost BMI (Multiprecision)
    inline void bmi_to_bytes(BN bigi, std::vector<uint8_t>& bytes) { auto byte_length = bytes.size(); while (byte_length != 0) { bytes[byte_length - 1] = static_cast<uint8_t>(0xff & bigi); bigi >>= 8; byte_length--; } }
    inline BN bytes_to_bmi(const std::vector<uint8_t>& bytes) { BN num = 0; for (auto byte: bytes) { num = num << 8 | byte; } return num; }
    BN read_input_large(uint256_t off, uint256_t len, Context* ctx);

    // BN Curve
    void precompile_bnpairing();
    void precompile_bnmul();
    void precompile_bnadd();

    // Instructions
    void stop();
    void add();
    void mul();
    void sub();
    void div();
    void sdiv();
    void mod();
    void addmod();
    void smod();
    void mulmod();
    void exp();
    void signextend();
    void lt();
    void gt();
    void slt();
    void sgt();
    void eq();
    void isZero();
    void and_();
    void or_();
    void xor_();
    void not_();
    void byte();
    void shl();
    void shr();
    void sar();
    void sha3();
    void address();
    void balance();
    void origin();
    void caller();
    void callvalue();
    void calldataload();
    void calldatasize();
    void calldatacopy();
    void codecopy();
    void gasprice();
    void extcodesize();
    void extcodecopy();
    void returndatasize();
    void returndatacopy();
    void extcodehash();
    void blockhash();
    void coinbase();
    void timestamp();
    void number();
    void difficulty();
    void gaslimit();
    void chainid();
    void selfbalance();
    void pop();
    void mload();
    void mstore();
    void mstore8();
    void codesize();
    void sload();
    void sstore();
    void jump();
    void jumpi();
    void pc();
    void msize();
    void gas();
    void jumpdest();
    void push();
    void dup();
    void swap();
    void log();
    void create();
    uint256_t value_by_call_type(const unsigned char call_type);
    void call();
    void return_();
    void revert();
    void selfdestruct();
    void invalid();
    void illegal();
  };
}


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

#ifndef BOOST_SIMD_SWAR_FUNCTIONS_CUMPROD_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_CUMPROD_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>
#include <boost/simd/operator/functions/multiplies.hpp>
#include <boost/simd/constant/constants/one.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  cumprod generic tag

      Represents the cumprod function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct cumprod_ : ext::cumulative_<cumprod_, tag::multiplies_, tag::One>
    {
      /// @brief Parent hierarchy
      typedef ext::cumulative_<cumprod_, tag::multiplies_, tag::One> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_cumprod_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::cumprod_, Site> dispatching_cumprod_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::cumprod_, Site>();
    }
    template<class... Args>
    struct impl_cumprod_;
  }
  /*!
    Computes the cumulated product of the vector elements

    Take care that overflow is very easy to get here especially for integral types
    char and unsigned char always overflow except for 0 and 1 !

    @par semantic:
    For any given vector @c x of type @c T:

    @code
    T r = cumprod(x);
    @endcode

    The function coincide with plus for floating point parameters.
    For integers is similar to:

    @code
    T r =x;
    for(int i=0;i < T::static_size; ++i)
      r[i]*= r[i-1];
    @endcode

    @param  a0

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumprod_, cumprod, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::cumprod_, cumprod, 2)
} }

#endif


// Copyright (C) 2004-2006 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine

/**
 * This header implements four distributed algorithms to compute
 * the minimum spanning tree (actually, minimum spanning forest) of a
 * graph. All of the algorithms were implemented as specified in the
 * paper by Dehne and Gotz:
 *
 *   Frank Dehne and Silvia Gotz. Practical Parallel Algorithms for Minimum
 *   Spanning Trees. In Symposium on Reliable Distributed Systems,
 *   pages 366--371, 1998.
 *
 * There are four algorithm variants implemented.
 */

#ifndef BOOST_DEHNE_GOTZ_MIN_SPANNING_TREE_HPP
#define BOOST_DEHNE_GOTZ_MIN_SPANNING_TREE_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <vector>
#include <boost/graph/parallel/algorithm.hpp>
#include <boost/limits.hpp>
#include <utility>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/property_map/parallel/caching_property_map.hpp>
#include <boost/graph/vertex_and_edge_range.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/graph/parallel/container_traits.hpp>
#include <boost/graph/parallel/detail/untracked_pair.hpp>
#include <cmath>

namespace boost { namespace graph { namespace distributed {

namespace detail {
  /**
   * Binary function object type that selects the (edge, weight) pair
   * with the minimum weight. Used within a Boruvka merge step to select
   * the candidate edges incident to each supervertex.
   */
  struct smaller_weighted_edge
  {
    template<typename Edge, typename Weight>
    std::pair<Edge, Weight>
    operator()(const std::pair<Edge, Weight>& x,
               const std::pair<Edge, Weight>& y) const
    { return x.second < y.second? x : y; }
  };

  /**
   * Unary predicate that determines if the source and target vertices
   * of the given edge have the same representative within a disjoint
   * sets data structure. Used to indicate when an edge is now a
   * self-loop because of supervertex merging in Boruvka's algorithm.
   */
  template<typename DisjointSets, typename Graph>
  class do_has_same_supervertex
  {
  public:
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

    do_has_same_supervertex(DisjointSets& dset, const Graph& g)
      : dset(dset), g(g) { }

    bool operator()(edge_descriptor e)
    { return dset.find_set(source(e, g)) == dset.find_set(target(e, g));    }

  private:
    DisjointSets&  dset;
    const Graph&   g;
  };

  /**
   * Build a @ref do_has_same_supervertex object.
   */
  template<typename DisjointSets, typename Graph>
  inline do_has_same_supervertex<DisjointSets, Graph>
  has_same_supervertex(DisjointSets& dset, const Graph& g)
  { return do_has_same_supervertex<DisjointSets, Graph>(dset, g); }

  /** \brief A single distributed Boruvka merge step.
   *
   * A distributed Boruvka merge step involves computing (globally)
   * the minimum weight edges incident on each supervertex and then
   * merging supervertices along these edges. Once supervertices are
   * merged, self-loops are eliminated.
   *
   * The set of parameters passed to this algorithm is large, and
   * considering this algorithm in isolation there are several
   * redundancies. However, the more asymptotically efficient
   * distributed MSF algorithms require mixing Boruvka steps with the
   * merging of local MSFs (implemented in
   * merge_local_minimum_spanning_trees_step): the interaction of the
   * two algorithms mandates the addition of these parameters.
   *
   * \param pg The process group over which communication should be
   * performed. Within the distributed Boruvka algorithm, this will be
   * equivalent to \code process_group(g); however, in the context of
   * the mixed MSF algorithms, the process group @p pg will be a
   * (non-strict) process subgroup of \code process_group(g).
   *
   * \param g The underlying graph on which the MSF is being
   * computed. The type of @p g must model DistributedGraph, but there
   * are no other requirements because the edge and (super)vertex
   * lists are passed separately.
   *
   * \param weight_map Property map containing the weights of each
   * edge. The type of this property map must model
   * ReadablePropertyMap and must support caching.
   *
   * \param out An output iterator that will be written with the set
   * of edges selected to build the MSF. Every process within the
   * process group @p pg will receive all edges in the MSF.
   *
   * \param dset Disjoint sets data structure mapping from vertices in
   * the graph @p g to their representative supervertex.
   *
   * \param supervertex_map Mapping from supervertex descriptors to
   * indices.
   *
   * \param supervertices A vector containing all of the
   * supervertices. Will be modified to include only the remaining
   * supervertices after merging occurs.
   *
   * \param edge_list The list of edges that remain in the graph. This
   * list will be pruned to remove self-loops once MSF edges have been
   * found.
   */
  template<typename ProcessGroup, typename Graph, typename WeightMap,
           typename OutputIterator, typename RankMap, typename ParentMap,
           typename SupervertexMap, typename Vertex, typename EdgeList>
  OutputIterator
  boruvka_merge_step(ProcessGroup pg, const Graph& g, WeightMap weight_map,
                     OutputIterator out,
                     disjoint_sets<RankMap, ParentMap>& dset,
                     SupervertexMap supervertex_map,
                     std::vector<Vertex>& supervertices,
                     EdgeList& edge_list)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor
                                                           vertex_descriptor;
    typedef typename graph_traits<Graph>::vertices_size_type
                                                           vertices_size_type;
    typedef typename graph_traits<Graph>::edge_descriptor  edge_descriptor;
    typedef typename EdgeList::iterator                    edge_iterator;
    typedef typename property_traits<WeightMap>::value_type
                                                           weight_type;
    typedef boost::parallel::detail::untracked_pair<edge_descriptor,
                                       weight_type>        w_edge;
    typedef typename property_traits<SupervertexMap>::value_type
                                                           supervertex_index;

    smaller_weighted_edge min_edge;
    weight_type inf = (std::numeric_limits<weight_type>::max)();

    // Renumber the supervertices
    for (std::size_t i = 0; i < supervertices.size(); ++i)
      put(supervertex_map, supervertices[i], i);

    // BSP-B1: Find local minimum-weight edges for each supervertex
    std::vector<w_edge> candidate_edges(supervertices.size(),
                                        w_edge(edge_descriptor(), inf));
    for (edge_iterator ei = edge_list.begin(); ei != edge_list.end(); ++ei) {
      weight_type w = get(weight_map, *ei);
      supervertex_index u =
        get(supervertex_map, dset.find_set(source(*ei, g)));
      supervertex_index v =
        get(supervertex_map, dset.find_set(target(*ei, g)));

      if (u != v) {
        candidate_edges[u] = min_edge(candidate_edges[u], w_edge(*ei, w));
        candidate_edges[v] = min_edge(candidate_edges[v], w_edge(*ei, w));
      }
    }

    // BSP-B2 (a): Compute global minimum edges for each supervertex
    all_reduce(pg,
               &candidate_edges[0],
               &candidate_edges[0] + candidate_edges.size(),
               &candidate_edges[0], min_edge);

    // BSP-B2 (b): Use the edges to compute sequentially the new
    // connected components and emit the edges.
    for (vertices_size_type i = 0; i < candidate_edges.size(); ++i) {
      if (candidate_edges[i].second != inf) {
        edge_descriptor e = candidate_edges[i].first;
        vertex_descriptor u = dset.find_set(source(e, g));
        vertex_descriptor v = dset.find_set(target(e, g));
        if (u != v) {
          // Emit the edge, but cache the weight so everyone knows it
          cache(weight_map, e, candidate_edges[i].second);
          *out++ = e;

          // Link the two supervertices
          dset.link(u, v);

          // Whichever vertex was reparented will be removed from the
          // list of supervertices.
          vertex_descriptor victim = u;
          if (dset.find_set(u) == u) victim = v;
          supervertices[get(supervertex_map, victim)] =
            graph_traits<Graph>::null_vertex();
        }
      }
    }

    // BSP-B3: Eliminate self-loops
    edge_list.erase(std::remove_if(edge_list.begin(), edge_list.end(),
                                   has_same_supervertex(dset, g)),
                    edge_list.end());

    // TBD: might also eliminate multiple edges between supervertices
    // when the edges do not have the best weight, but this is not
    // strictly necessary.

    // Eliminate supervertices that have been absorbed
    supervertices.erase(std::remove(supervertices.begin(),
                                    supervertices.end(),
                                    graph_traits<Graph>::null_vertex()),
                        supervertices.end());

    return out;
  }

  /**
   * An edge descriptor adaptor that reroutes the source and target
   * edges to different vertices, but retains the original edge
   * descriptor for, e.g., property maps. This is used when we want to
   * turn a set of edges in the overall graph into a set of edges
   * between supervertices.
   */
  template<typename Graph>
  struct supervertex_edge_descriptor
  {
    typedef supervertex_edge_descriptor self_type;
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor Edge;

    Vertex source;
    Vertex target;
    Edge e;

    operator Edge() const { return e; }

    friend inline bool operator==(const self_type& x, const self_type& y)
    { return x.e == y.e; }

    friend inline bool operator!=(const self_type& x, const self_type& y)
    { return x.e != y.e; }
  };

  template<typename Graph>
  inline typename supervertex_edge_descriptor<Graph>::Vertex
  source(supervertex_edge_descriptor<Graph> se, const Graph&)
  { return se.source; }

  template<typename Graph>
  inline typename supervertex_edge_descriptor<Graph>::Vertex
  target(supervertex_edge_descriptor<Graph> se, const Graph&)
  { return se.target; }

  /**
   * Build a supervertex edge descriptor from a normal edge descriptor
   * using the given disjoint sets data structure to identify
   * supervertex representatives.
   */
  template<typename Graph, typename DisjointSets>
  struct build_supervertex_edge_descriptor
  {
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor   Edge;

    typedef Edge argument_type;
    typedef supervertex_edge_descriptor<Graph> result_type;

    build_supervertex_edge_descriptor() : g(0), dsets(0) { }

    build_supervertex_edge_descriptor(const Graph& g, DisjointSets& dsets)
      : g(&g), dsets(&dsets) { }

    result_type operator()(argument_type e) const
    {
      result_type result;
      result.source = dsets->find_set(source(e, *g));
      result.target = dsets->find_set(target(e, *g));
      result.e = e;
      return result;
    }

  private:
    const Graph* g;
    DisjointSets* dsets;
  };

  template<typename Graph, typename DisjointSets>
  inline build_supervertex_edge_descriptor<Graph, DisjointSets>
  make_supervertex_edge_descriptor(const Graph& g, DisjointSets& dsets)
  { return build_supervertex_edge_descriptor<Graph, DisjointSets>(g, dsets); }

  template<typename T>
  struct identity_function
  {
    typedef T argument_type;
    typedef T result_type;

    result_type operator()(argument_type x) const { return x; }
  };

  template<typename Graph, typename DisjointSets, typename EdgeMapper>
  class is_not_msf_edge
  {
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename graph_traits<Graph>::edge_descriptor Edge;

  public:
    is_not_msf_edge(const Graph& g, DisjointSets dset, EdgeMapper edge_mapper)
      : g(g), dset(dset), edge_mapper(edge_mapper) { }

    bool operator()(Edge e)
    {
      Vertex u = dset.find_set(source(edge_mapper(e), g));
      Vertex v = dset.find_set(target(edge_mapper(e), g));
      if (u == v) return true;
      else {
        dset.link(u, v);
        return false;
      }
    }

  private:
    const Graph& g;
    DisjointSets dset;
    EdgeMapper edge_mapper;
  };

  template<typename Graph, typename ForwardIterator, typename EdgeList,
           typename EdgeMapper, typename RankMap, typename ParentMap>
  void
  sorted_mutating_kruskal(const Graph& g,
                          ForwardIterator first_vertex,
                          ForwardIterator last_vertex,
                          EdgeList& edge_list, EdgeMapper edge_mapper,
                          RankMap rank_map, ParentMap parent_map)
  {
    typedef disjoint_sets<RankMap, ParentMap> DisjointSets;

    // Build and initialize disjoint-sets data structure
    DisjointSets dset(rank_map, parent_map);
    for (ForwardIterator v = first_vertex; v != last_vertex; ++v)
      dset.make_set(*v);

    is_not_msf_edge<Graph, DisjointSets, EdgeMapper>
      remove_non_msf_edges(g, dset, edge_mapper);
    edge_list.erase(std::remove_if(edge_list.begin(), edge_list.end(),
                                   remove_non_msf_edges),
                    edge_list.end());
  }

  /**
   * Merge local minimum spanning forests from p processes into
   * minimum spanning forests on p/D processes (where D is the tree
   * factor, currently fixed at 3), eliminating unnecessary edges in
   * the process.
   *
   * As with @ref boruvka_merge_step, this routine has many
   * parameters, not all of which make sense within the limited
   * context of this routine. The parameters are required for the
   * Boruvka and local MSF merging steps to interoperate.
   *
   * \param pg The process group on which local minimum spanning
   * forests should be merged. The top (D-1)p/D processes will be
   * eliminated, and a new process subgroup containing p/D processors
   * will be returned. The value D is a constant factor that is
   * currently fixed to 3.
   *
   * \param g The underlying graph whose MSF is being computed. It must model
   * the DistributedGraph concept.
   *
   * \param first_vertex Iterator to the first vertex in the graph
   * that should be considered. While the local MSF merging algorithm
   * typically operates on the entire vertex set, within the hybrid
   * distributed MSF algorithms this will refer to the first
   * supervertex.
   *
   * \param last_vertex The past-the-end iterator for the vertex list.
   *
   * \param edge_list The list of local edges that will be
   * considered. For the p/D processes that remain, this list will
   * contain edges in the MSF known to the vertex after other
   * processes' edge lists have been merged. The edge list must be
   * sorted in order of increasing weight.
   *
   * \param weight Property map containing the weights of each
   * edge. The type of this property map must model
   * ReadablePropertyMap and must support caching.
   *
   * \param global_index Mapping from vertex descriptors to a global
   * index. The type must model ReadablePropertyMap.
   *
   * \param edge_mapper A function object that can remap edge descriptors
   * in the edge list to any alternative edge descriptor. This
   * function object will be the identity function when a pure merging
   * of local MSFs is required, but may be a mapping to a supervertex
   * edge when the local MSF merging occurs on a supervertex
   * graph. This function object saves us the trouble of having to
   * build a supervertex graph adaptor.
   *
   * \param already_local_msf True when the edge list already
   * constitutes a local MSF. If false, Kruskal's algorithm will first
   * be applied to the local edge list to select MSF edges.
   *
   * \returns The process subgroup containing the remaining p/D
   * processes. If the size of this process group is greater than one,
   * the MSF edges contained in the edge list do not constitute an MSF
   * for the entire graph.
   */
  template<typename ProcessGroup, typename Graph, typename ForwardIterator,
           typename EdgeList, typename WeightMap, typename GlobalIndexMap,
           typename EdgeMapper>
  ProcessGroup
  merge_local_minimum_spanning_trees_step(ProcessGroup pg,
                                          const Graph& g,
                                          ForwardIterator first_vertex,
                                          ForwardIterator last_vertex,
                                          EdgeList& edge_list,
                                          WeightMap weight,
                                          GlobalIndexMap global_index,
                                          EdgeMapper edge_mapper,
                                          bool already_local_msf)
  {
    typedef typename ProcessGroup::process_id_type process_id_type;
    typedef typename EdgeList::value_type edge_descriptor;
    typedef typename property_traits<WeightMap>::value_type weight_type;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;

    // The tree factor, often called "D"
    process_id_type const tree_factor = 3;
    process_id_type num_procs = num_processes(pg);
    process_id_type id = process_id(pg);
    process_id_type procs_left = (num_procs + tree_factor - 1) / tree_factor;
    std::size_t n = std::size_t(last_vertex - first_vertex);

    if (!already_local_msf) {
      // Compute local minimum spanning forest. We only care about the
      // edges in the MSF, because only edges in the local MSF can be in
      // the global MSF.
      std::vector<std::size_t> ranks(n);
      std::vector<vertex_descriptor> parents(n);
      detail::sorted_mutating_kruskal
        (g, first_vertex, last_vertex,
         edge_list, edge_mapper,
         make_iterator_property_map(ranks.begin(), global_index),
         make_iterator_property_map(parents.begin(), global_index));
    }

    typedef std::pair<edge_descriptor, weight_type> w_edge;

    // Order edges based on their weights.
    indirect_cmp<WeightMap, std::less<weight_type> > cmp_edge_weight(weight);

    if (id < procs_left) {
      // The p/D processes that remain will receive local MSF edges from
      // D-1 other processes.
      synchronize(pg);
      for (process_id_type from_id = procs_left + id; from_id < num_procs;
           from_id += procs_left) {
        std::size_t num_incoming_edges;
        receive(pg, from_id, 0, num_incoming_edges);
        if (num_incoming_edges > 0) {
          std::vector<w_edge> incoming_edges(num_incoming_edges);
          receive(pg, from_id, 1, &incoming_edges[0], num_incoming_edges);

          edge_list.reserve(edge_list.size() + num_incoming_edges);
          for (std::size_t i = 0; i < num_incoming_edges; ++i) {
            cache(weight, incoming_edges[i].first, incoming_edges[i].second);
            edge_list.push_back(incoming_edges[i].first);
          }
          std::inplace_merge(edge_list.begin(),
                             edge_list.end() - num_incoming_edges,
                             edge_list.end(),
                             cmp_edge_weight);
        }
      }

      // Compute the local MSF from union of the edges in the MSFs of
      // all children.
      std::vector<std::size_t> ranks(n);
      std::vector<vertex_descriptor> parents(n);
      detail::sorted_mutating_kruskal
        (g, first_vertex, last_vertex,
         edge_list, edge_mapper,
         make_iterator_property_map(ranks.begin(), global_index),
         make_iterator_property_map(parents.begin(), global_index));
    } else {
      // The (D-1)p/D processes that are dropping out of further
      // computations merely send their MSF edges to their parent
      // process in the process tree.
      send(pg, id % procs_left, 0, edge_list.size());
      if (edge_list.size() > 0) {
        std::vector<w_edge> outgoing_edges;
        outgoing_edges.reserve(edge_list.size());
        for (std::size_t i = 0; i < edge_list.size(); ++i) {
          outgoing_edges.push_back(std::make_pair(edge_list[i],
                                                  get(weight, edge_list[i])));
        }
        send(pg, id % procs_left, 1, &outgoing_edges[0],
             outgoing_edges.size());
      }
      synchronize(pg);
    }

    // Return a process subgroup containing the p/D parent processes
    return process_subgroup(pg,
                            make_counting_iterator(process_id_type(0)),
                            make_counting_iterator(procs_left));
  }
} // end namespace detail

// ---------------------------------------------------------------------
// Dense Boruvka MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename VertexIndexMap, typename RankMap, typename ParentMap,
         typename SupervertexMap>
OutputIterator
dense_boruvka_minimum_spanning_tree(const Graph& g, WeightMap weight_map,
                                    OutputIterator out,
                                    VertexIndexMap index_map,
                                    RankMap rank_map, ParentMap parent_map,
                                    SupervertexMap supervertex_map)
{
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor  vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator    vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor    edge_descriptor;

  // Don't throw away cached edge weights
  weight_map.set_max_ghost_cells(0);

  // Initialize the disjoint sets structures
  disjoint_sets<RankMap, ParentMap> dset(rank_map, parent_map);
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    dset.make_set(*vi);

  std::vector<vertex_descriptor> supervertices;
  supervertices.assign(vertices(g).first, vertices(g).second);

  // Use Kruskal's algorithm to find the minimum spanning forest
  // considering only the local edges. The resulting edges are not
  // necessarily going to be in the final minimum spanning
  // forest. However, any edge not part of the local MSF cannot be a
  // part of the global MSF, so we should have eliminated some edges
  // from consideration.
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight_map).
     vertex_index_map(index_map));

  // While the number of supervertices is decreasing, keep executing
  // Boruvka steps to identify additional MSF edges. This loop will
  // execute log |V| times.
  vertices_size_type old_num_supervertices;
  do {
    old_num_supervertices = supervertices.size();
    out = detail::boruvka_merge_step(process_group(g), g,
                                     weight_map, out,
                                     dset, supervertex_map, supervertices,
                                     edge_list);
  } while (supervertices.size() < old_num_supervertices);

  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator,
         typename VertexIndex>
OutputIterator
dense_boruvka_minimum_spanning_tree(const Graph& g, WeightMap weight_map,
                                    OutputIterator out, VertexIndex i_map)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;

  std::vector<std::size_t> ranks(num_vertices(g));
  std::vector<vertex_descriptor> parents(num_vertices(g));
  std::vector<std::size_t> supervertices(num_vertices(g));

  return dense_boruvka_minimum_spanning_tree
           (g, weight_map, out, i_map,
            make_iterator_property_map(ranks.begin(), i_map),
            make_iterator_property_map(parents.begin(), i_map),
            make_iterator_property_map(supervertices.begin(), i_map));
}

template<typename Graph, typename WeightMap, typename OutputIterator>
OutputIterator
dense_boruvka_minimum_spanning_tree(const Graph& g, WeightMap weight_map,
                                    OutputIterator out)
{
  return dense_boruvka_minimum_spanning_tree(g, weight_map, out,
                                             get(vertex_index, g));
}

// ---------------------------------------------------------------------
// Merge local MSFs MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap>
OutputIterator
merge_local_minimum_spanning_trees(const Graph& g, WeightMap weight,
                                   OutputIterator out,
                                   GlobalIndexMap global_index)
{
  using boost::graph::parallel::process_group_type;
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;

  // Don't throw away cached edge weights
  weight.set_max_ghost_cells(0);

  // Compute the initial local minimum spanning forests
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight).vertex_index_map(global_index));

  // Merge the local MSFs from p processes into p/D processes,
  // reducing the number of processes in each step. Continue looping
  // until either (a) the current process drops out or (b) only one
  // process remains in the group. This loop will execute log_D p
  // times.
  typename process_group_type<Graph>::type pg = process_group(g);
  while (pg && num_processes(pg) > 1) {
    pg = detail::merge_local_minimum_spanning_trees_step
           (pg, g, vertices(g).first, vertices(g).second,
            edge_list, weight, global_index,
            detail::identity_function<edge_descriptor>(), true);
  }

  // Only process 0 has the entire edge list, so emit it to the output
  // iterator.
  if (pg && process_id(pg) == 0) {
    out = std::copy(edge_list.begin(), edge_list.end(), out);
  }

  synchronize(process_group(g));
  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator>
inline OutputIterator
merge_local_minimum_spanning_trees(const Graph& g, WeightMap weight,
                                   OutputIterator out)
{
  return merge_local_minimum_spanning_trees(g, weight, out,
                                            get(vertex_index, g));
}

// ---------------------------------------------------------------------
// Boruvka-then-merge MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap, typename RankMap, typename ParentMap,
         typename SupervertexMap>
OutputIterator
boruvka_then_merge(const Graph& g, WeightMap weight, OutputIterator out,
                   GlobalIndexMap index, RankMap rank_map,
                   ParentMap parent_map, SupervertexMap supervertex_map)
{
  using std::log;
  using boost::graph::parallel::process_group_type;
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor  vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator    vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor    edge_descriptor;

  // Don't throw away cached edge weights
  weight.set_max_ghost_cells(0);

  // Compute the initial local minimum spanning forests
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight).
     vertex_index_map(index));

  // Initialize the disjoint sets structures for Boruvka steps
  disjoint_sets<RankMap, ParentMap> dset(rank_map, parent_map);
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    dset.make_set(*vi);

  // Construct the initial set of supervertices (all vertices)
  std::vector<vertex_descriptor> supervertices;
  supervertices.assign(vertices(g).first, vertices(g).second);

  // Continue performing Boruvka merge steps until the number of
  // supervertices reaches |V| / (log_D p)^2.
  const std::size_t tree_factor = 3; // TBD: same as above! should be param
  double log_d_p = log((double)num_processes(process_group(g)))
                 / log((double)tree_factor);
  vertices_size_type target_supervertices =
    vertices_size_type(num_vertices(g) / (log_d_p * log_d_p));
  vertices_size_type old_num_supervertices;
  while (supervertices.size() > target_supervertices) {
    old_num_supervertices = supervertices.size();
    out = detail::boruvka_merge_step(process_group(g), g,
                                     weight, out, dset,
                                     supervertex_map, supervertices,
                                     edge_list);
    if (supervertices.size() == old_num_supervertices)
      return out;
  }

  // Renumber the supervertices
  for (std::size_t i = 0; i < supervertices.size(); ++i)
    put(supervertex_map, supervertices[i], i);

  // Merge local MSFs on the supervertices. (D-1)p/D processors drop
  // out each iteration, so this loop executes log_D p times.
  typename process_group_type<Graph>::type pg = process_group(g);
  bool have_msf = false;
  while (pg && num_processes(pg) > 1) {
    pg = detail::merge_local_minimum_spanning_trees_step
           (pg, g, supervertices.begin(), supervertices.end(),
            edge_list, weight, supervertex_map,
            detail::make_supervertex_edge_descriptor(g, dset),
            have_msf);
    have_msf = true;
  }

  // Only process 0 has the complete list of _supervertex_ MST edges,
  // so emit those to the output iterator. This is not the complete
  // list of edges in the MSF, however: the Boruvka steps in the
  // beginning of the algorithm emitted any edges used to merge
  // supervertices.
  if (pg && process_id(pg) == 0)
    out = std::copy(edge_list.begin(), edge_list.end(), out);

  synchronize(process_group(g));
  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap>
inline OutputIterator
boruvka_then_merge(const Graph& g, WeightMap weight, OutputIterator out,
                    GlobalIndexMap index)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  std::vector<vertices_size_type> ranks(num_vertices(g));
  std::vector<vertex_descriptor> parents(num_vertices(g));
  std::vector<vertices_size_type> supervertex_indices(num_vertices(g));

  return boruvka_then_merge
           (g, weight, out, index,
            make_iterator_property_map(ranks.begin(), index),
            make_iterator_property_map(parents.begin(), index),
            make_iterator_property_map(supervertex_indices.begin(), index));
}

template<typename Graph, typename WeightMap, typename OutputIterator>
inline OutputIterator
boruvka_then_merge(const Graph& g, WeightMap weight, OutputIterator out)
{ return boruvka_then_merge(g, weight, out, get(vertex_index, g)); }

// ---------------------------------------------------------------------
// Boruvka-mixed-merge MSF algorithm
// ---------------------------------------------------------------------
template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap, typename RankMap, typename ParentMap,
         typename SupervertexMap>
OutputIterator
boruvka_mixed_merge(const Graph& g, WeightMap weight, OutputIterator out,
                    GlobalIndexMap index, RankMap rank_map,
                    ParentMap parent_map, SupervertexMap supervertex_map)
{
  using boost::graph::parallel::process_group_type;
  using boost::graph::parallel::process_group;

  typedef typename graph_traits<Graph>::traversal_category traversal_category;

  BOOST_STATIC_ASSERT((is_convertible<traversal_category*,
                                      vertex_list_graph_tag*>::value));

  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  typedef typename graph_traits<Graph>::vertex_descriptor  vertex_descriptor;
  typedef typename graph_traits<Graph>::vertex_iterator    vertex_iterator;
  typedef typename graph_traits<Graph>::edge_descriptor    edge_descriptor;

  // Don't throw away cached edge weights
  weight.set_max_ghost_cells(0);

  // Initialize the disjoint sets structures for Boruvka steps
  disjoint_sets<RankMap, ParentMap> dset(rank_map, parent_map);
  vertex_iterator vi, vi_end;
  for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    dset.make_set(*vi);

  // Construct the initial set of supervertices (all vertices)
  std::vector<vertex_descriptor> supervertices;
  supervertices.assign(vertices(g).first, vertices(g).second);

  // Compute the initial local minimum spanning forests
  std::vector<edge_descriptor> edge_list;
  kruskal_minimum_spanning_tree
    (make_vertex_and_edge_range(g, vertices(g).first, vertices(g).second,
                                edges(g).first, edges(g).second),
     std::back_inserter(edge_list),
     boost::weight_map(weight).
     vertex_index_map(index));

  if (num_processes(process_group(g)) == 1) {
    return std::copy(edge_list.begin(), edge_list.end(), out);
  }

  // Like the merging local MSFs algorithm and the Boruvka-then-merge
  // algorithm, each iteration of this loop reduces the number of
  // processes by a constant factor D, and therefore we require log_D
  // p iterations. Note also that the number of edges in the edge list
  // decreases geometrically, giving us an efficient distributed MSF
  // algorithm.
  typename process_group_type<Graph>::type pg = process_group(g);
  vertices_size_type old_num_supervertices;
  while (pg && num_processes(pg) > 1) {
    // A single Boruvka step. If this doesn't change anything, we're done
    old_num_supervertices = supervertices.size();
    out = detail::boruvka_merge_step(pg, g, weight, out, dset,
                                     supervertex_map, supervertices,
                                     edge_list);
    if (old_num_supervertices == supervertices.size()) {
      edge_list.clear();
      break;
    }

    // Renumber the supervertices
    for (std::size_t i = 0; i < supervertices.size(); ++i)
      put(supervertex_map, supervertices[i], i);

    // A single merging of local MSTs, which reduces the number of
    // processes we're using by a constant factor D.
    pg = detail::merge_local_minimum_spanning_trees_step
           (pg, g, supervertices.begin(), supervertices.end(),
            edge_list, weight, supervertex_map,
            detail::make_supervertex_edge_descriptor(g, dset),
            true);

  }

  // Only process 0 has the complete edge list, so emit it for the
  // user. Note that list edge list only contains the MSF edges in the
  // final supervertex graph: all of the other edges were used to
  // merge supervertices and have been emitted by the Boruvka steps,
  // although only process 0 has received the complete set.
  if (pg && process_id(pg) == 0)
    out = std::copy(edge_list.begin(), edge_list.end(), out);

  synchronize(process_group(g));
  return out;
}

template<typename Graph, typename WeightMap, typename OutputIterator,
         typename GlobalIndexMap>
inline OutputIterator
boruvka_mixed_merge(const Graph& g, WeightMap weight, OutputIterator out,
                    GlobalIndexMap index)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
  std::vector<vertices_size_type> ranks(num_vertices(g));
  std::vector<vertex_descriptor> parents(num_vertices(g));
  std::vector<vertices_size_type> supervertex_indices(num_vertices(g));

  return boruvka_mixed_merge
           (g, weight, out, index,
            make_iterator_property_map(ranks.begin(), index),
            make_iterator_property_map(parents.begin(), index),
            make_iterator_property_map(supervertex_indices.begin(), index));
}

template<typename Graph, typename WeightMap, typename OutputIterator>
inline OutputIterator
boruvka_mixed_merge(const Graph& g, WeightMap weight, OutputIterator out)
{ return boruvka_mixed_merge(g, weight, out, get(vertex_index, g)); }

} // end namespace distributed

using distributed::dense_boruvka_minimum_spanning_tree;
using distributed::merge_local_minimum_spanning_trees;
using distributed::boruvka_then_merge;
using distributed::boruvka_mixed_merge;

} } // end namespace boost::graph

#endif // BOOST_DEHNE_GOTZ_MIN_SPANNING_TREE_HPP


/*******************************************************************************
*
*  Filename    : CombinePull.
*  Description : implementing combine pull Functions
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/LimitCalc/interface/CombinePull.hpp"
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"

#include "ManagerUtils/Common/interface/ConfigReader.hpp"
#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/SysUtils/interface/HiggsCombineSubmitter.hpp"
#include "ManagerUtils/SysUtils/interface/PathUtils.hpp"
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"

#include <boost/format.hpp>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <utility>
#include <vector>

#include "RooFitResult.h"
#include "RooRealVar.h"
#include "TFile.h"

using namespace std;
using mgr::Parameter;

/******************************************************************************/

void
MakeCombinePull()
{
  const mgr::ConfigReader higgscfg( limnamer.SettingsDir() / "higgs_combine_settings.json" );

  const string cardfile = limnamer.TextFileName( "card" );

  const string scriptfilename = limnamer.TextFileName( "combpull" );

  const string storeroot = limnamer.RootFileName( "combpull" );

  cout << cardfile << endl;

  ofstream scriptfile( scriptfilename );

  scriptfile << "#!/bin/bash" << endl;

  scriptfile << boost::format( "cd %s/%s/src" )
    % higgscfg.GetStaticString( "Store Path" )
    % higgscfg.GetStaticString( "CMSSW Version" )
             << endl;

  scriptfile <<  "eval `scramv1 runtime -sh 2> /dev/null`" << endl;

  scriptfile << boost::format( "combine -M MaxLikelihoodFit %s" ) % cardfile << endl;

  scriptfile << boost::format( "cp mlfit.root %s" ) % storeroot << endl;

  scriptfile.close();

  system( ( "chmod +x " + scriptfilename ).c_str() );
  system( scriptfilename.c_str() );

  system( "sleep 5" );
}

/******************************************************************************/

static Parameter
MakeVarPull( const RooArgList& post, const RooArgList& pre, const string& name )
{
  vector<RooRealVar*> postvarlist;
  vector<RooRealVar*> prevarlist;

  for( int i = 0; i < post.getSize(); ++i ){
    RooRealVar* postvar = (RooRealVar*)( post.at( i ) );
    if( std::string( postvar->GetName() ).find( name ) != string::npos ){
      postvarlist.push_back( postvar );
    }
  }

  for( int i = 0; i < pre.getSize(); ++i ){
    RooRealVar* prevar = (RooRealVar*)( pre.at( i ) );
    if( std::string( prevar->GetName() ).find( name ) != string::npos ){
      prevarlist.push_back( prevar );
    }
  }

  if( postvarlist.size() && prevarlist.size() ){
    const double preval  = prevarlist.front()->getVal();
    const double preerr  = prevarlist.front()->getError();
    const double postval = postvarlist.front()->getVal();
    const double posterr = postvarlist.front()->getError();
    const double pull    = ( postval - preval ) / preerr;
    const double pullerr = posterr / preerr;
    return Parameter( pull, pullerr, pullerr );
  } else {
    cout << "Warning! parameter [" << name  << "] not found!" << endl;
    return Parameter( 0, 0, 0 );
  }
}

/******************************************************************************/

void
MakePullPlot()
{
  TFile* file               = TFile::Open( limnamer.RootFileName( "combpull" ).c_str(), "READ" );
  const RooArgList& postfit = ( (RooFitResult*)( file->Get( "fit_s" ) ) )->floatParsFinal();
  const RooArgSet& prefit   = *( (RooArgSet*)( file->Get( "nuisances_prefit" ) ) );

  vector<pair<string, Parameter> > pulllist;

  // Fitting paramter varibles
  // const string avarname   = limnamer.GetInput<string>( "masspoint" ) + "simfitbga";
  // const string bvarname   = limnamer.GetInput<string>( "masspoint" ) + "simfitbgb";
  // const string sysvarname = limnamer.GetInput<string>( "masspoint" ) + "simfitbgsys";

  // pulllist.emplace_back( "Bkg. parm 1", MakeVarPull( postfit, prefit, avarname ) );
  // pulllist.emplace_back( "Bkg. parm 2", MakeVarPull( postfit, prefit, bvarname ) );
  // pulllist.emplace_back( "Bkg. sys",    MakeVarPull( postfit, prefit, sysvarname ) );

  // Shape uncertainties
  for( size_t i = 0; i < uncsource.size(); ++i ){
    boost::format shapename( "%s%s_%s%s%s" );
    const string upname = boost::str(
      shapename
      % limnamer.GetInput<string>( "masspoint" ) % "keymastercoeff"
      % uncsource.at( i )  % "Up"     %"key"
      );
    const string dwname = boost::str(
      shapename
      % limnamer.GetInput<string>( "masspoint" ) % "keymastercoeff"
      % uncsource.at( i ) % "Down" %"key"
      );

    const Parameter uppull = MakeVarPull( postfit, prefit, upname );
    const Parameter dwpull = MakeVarPull( postfit, prefit, dwname );

    const Parameter pull(
      uppull.CentralValue() + dwpull.CentralValue(),
      ( uppull.AbsUpperError() + dwpull.AbsUpperError() ) /2,
      ( uppull.AbsUpperError() + dwpull.AbsUpperError() ) /2
      );

    pulllist.emplace_back( uncsource.at( i ), pull );
  }

  // Misc options
  pulllist.emplace_back( "lep sys.", MakeVarPull( postfit, prefit, "lepsys" ) );
  pulllist.emplace_back( "lumi.",    MakeVarPull( postfit, prefit, "lumi" ) );
  pulllist.emplace_back( "stat.",    MakeVarPull( postfit, prefit, "statunc" ) );

  for( const auto& mypair : pulllist ){
    cout << mypair.first << " " << mgr::FloatingPoint( mypair.second.CentralValue(), 10 ) << endl;
  }

  delete file;

  // Beginning plotting
  TCanvas* c = mgr::NewCanvas();

  TH1D* pullhist = new TH1D( "pull", "pull", pulllist.size(), 0, pulllist.size() );

  for( size_t i = 0; i < pulllist.size(); ++i ){
    const string name    = pulllist.at( i ).first;
    const Parameter pull = pulllist.at( i ).second;
    pullhist->GetXaxis()->SetBinLabel( i+1, name.c_str() );
    pullhist->SetBinContent( i+1, pull.CentralValue() );
    pullhist->SetBinError( i+1, pull.AbsAvgError() );
  }

  mgr::SetSinglePad( c );
  c->SetBottomMargin( 1.5 * PLOT_Y_MIN );

  const double ymax = std::max( mgr::GetYmax( pullhist ), 2.0 );
  mgr::SetAxis( pullhist );
  tstar::SetDataStyle( pullhist );
  pullhist->Draw( PS_DATA );
  pullhist->SetTitle( "" );
  pullhist->SetStats( 0 );
  pullhist->GetYaxis()->SetTitle( "Post-fit pull" );
  pullhist->SetMaximum( ymax * 1.2 );
  pullhist->SetMinimum( ymax * -1.2 );

  mgr::DrawCMSLabel( PRELIMINARY );
  mgr::DrawLuminosity( limnamer.GetExt<double>( "era", "Lumi" ) );

  const double xmin = 0;
  const double xmax = pulllist.size();
  TLine* cen        = new TLine( xmin, 0, xmax, 0 );
  TLine* midtop     = new TLine( xmin, 0.5, xmax, 0.5 );
  TLine* top        = new TLine( xmin, 1, xmax, 1 );
  TLine* midbot     = new TLine( xmin, -0.5, xmax, -0.5 );
  TLine* bot        = new TLine( xmin, -1, xmax, -1 );
  cen->Draw();
  midtop->Draw();
  top->Draw();
  midbot->Draw();
  bot->Draw();
  pullhist->Draw( PS_DATA PS_SAME );

  cen->SetLineColor( kGray );
  cen->SetLineStyle( 1 );
  cen->SetLineWidth( 3 );
  midtop->SetLineColor( KBLUE );
  midbot->SetLineColor( KBLUE );
  midtop->SetLineWidth( 3 );
  midbot->SetLineWidth( 3 );
  midtop->SetLineStyle( 2 );
  midbot->SetLineStyle( 2 );
  top->SetLineColor( KRED );
  bot->SetLineColor( KRED );
  top->SetLineWidth( 3 );
  bot->SetLineWidth( 3 );
  top->SetLineStyle( 3 );
  bot->SetLineStyle( 3 );

  boost::format massfmt( "M_{t*}=%dGeV/c^{2}" );
  const string masstag = boost::str( massfmt % GetInt( limnamer.GetInput<string>( "masspoint" ) ) );

  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX + TEXT_MARGIN/2, BOTTOM_LEFT )
  .WriteLine( limnamer.GetExt<string>( "channel", "Root Name" ) )
  .SetOrigin( PLOT_X_TEXT_MAX, PLOT_Y_TEXT_MAX, TOP_RIGHT )
  .WriteLine( masstag );

  mgr::SaveToPDF( c, limnamer.PlotFileName( "combpull" ) );

  delete c;
  delete pullhist;
}


/*
	This file is part of cpp-ethereum.

	cpp-ethereum 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.

	cpp-ethereum 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 cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file dagger.cpp
 * @author Gav Wood <i@gavwood.com>
 * @date 2014
 * Dashimoto test functions.
 */

#include <fstream>
#include <cpp-ethereum/utils/json_spirit/JsonSpiritHeaders.h>
#include <cpp-ethereum/libdevcore/CommonIO.h>
#include <libethashseal/Ethash.h>
#include <libethashseal/EthashAux.h>
#include <boost/test/unit_test.hpp>
#include <test/libtesteth/TestHelper.h>

using namespace std;
using namespace dev;
using namespace dev::eth;
using namespace dev::test;

namespace js = json_spirit;

using dev::operator <<;

BOOST_FIXTURE_TEST_SUITE(DashimotoTests, TestOutputHelper)

BOOST_AUTO_TEST_CASE(basic_test)
{
	string testPath = test::getTestPath();

	testPath += "/PoWTests";

	cnote << "Testing Proof of Work...";
	js::mValue v;
	string s = contentsString(testPath + "/ethash_tests.json");
	BOOST_REQUIRE_MESSAGE(s.length() > 0, "Contents of 'ethash_tests.json' is empty. Have you cloned the 'tests' repo branch develop?");
	js::read_string(s, v);
	for (auto& i: v.get_obj())
	{
		cnote << i.first;
		js::mObject& o = i.second.get_obj();
		vector<pair<string, string>> ss;
		BlockHeader header(fromHex(o["header"].get_str()), HeaderData);
		h256 headerHash(o["header_hash"].get_str());
		Nonce nonce(o["nonce"].get_str());
		BOOST_REQUIRE_EQUAL(headerHash, header.hash(WithoutSeal));
		BOOST_REQUIRE_EQUAL(nonce, Ethash::nonce(header));

		unsigned cacheSize(o["cache_size"].get_int());
		h256 cacheHash(o["cache_hash"].get_str());
		BOOST_REQUIRE_EQUAL(EthashAux::get()->light(Ethash::seedHash(header))->size, cacheSize);
		BOOST_REQUIRE_EQUAL(sha3(EthashAux::get()->light(Ethash::seedHash(header))->data()), cacheHash);

#if TEST_FULL
		unsigned fullSize(o["full_size"].get_int());
		h256 fullHash(o["full_hash"].get_str());
		BOOST_REQUIRE_EQUAL(EthashAux::get()->full(Ethash::seedHash(header))->size(), fullSize);
		BOOST_REQUIRE_EQUAL(sha3(EthashAux::get()->full(Ethash::seedHash(header))->data()), fullHash);
#endif

		h256 result(o["result"].get_str());
		EthashProofOfWork::Result r = EthashAux::eval(Ethash::seedHash(header), header.hash(WithoutSeal), Ethash::nonce(header));
		BOOST_REQUIRE_EQUAL(r.value, result);
		BOOST_REQUIRE_EQUAL(r.mixHash, Ethash::mixHash(header));
	}
}

BOOST_AUTO_TEST_SUITE_END()