//-----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
//-----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax join
#include <boost/test/unit_test.hpp>

#include <mitrax/join.hpp>
#include <mitrax/compare.hpp>

using boost::typeindex::type_id;
using boost::typeindex::type_id_runtime;
using namespace mitrax;
using namespace mitrax::literals;

template < typename T >
auto rt_id(T&& v){
	return boost::typeindex::type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = boost::typeindex::type_id< T >();

template < typename T, typename U >
constexpr bool equal(T const& a, U const& b){
	using std::abs;
	return abs(a - b) < 0.00001;
}

template < typename T, typename U >
constexpr bool matrix_equal(T const& a, U const& b){
	using std::abs;
	auto m = a - b;
	return std::any_of(m.begin(), m.end(), [](auto v){
		return v < 0.00001;
	});
}

constexpr auto ref3x3 = make_matrix(3_CS, 3_RS, {
	{0, 1, 2},
	{3, 4, 5},
	{6, 7, 8}
});

constexpr auto ref6x6 = make_matrix(6_CS, 6_RS, {
	{0, 1, 2, 0, 1, 2},
	{3, 4, 5, 3, 4, 5},
	{6, 7, 8, 6, 7, 8},
	{0, 1, 2, 0, 1, 2},
	{3, 4, 5, 3, 4, 5},
	{6, 7, 8, 6, 7, 8}
});

BOOST_AUTO_TEST_SUITE(suite_join)

BOOST_AUTO_TEST_CASE(test_join_ct){
	constexpr auto m1 = make_matrix(2_CS, 2_RS, {{0, 1}, {3, 4}});
	constexpr auto m2 = make_matrix(1_CS, 2_RS, {{2}, {5}});
	constexpr auto m3 = make_matrix(2_CS, 1_RS, {{6, 7}});
	constexpr auto m4 = make_matrix(1_CS, 1_RS, {{8}});

	constexpr auto m5 = join(join_v(
		join_h(m1, m2),
		join_h(m3, m4)
	));

	constexpr auto m6 = join(join_h(
		join_v(m1, m3),
		join_v(m2, m4)
	));

	constexpr auto m7 = join(
		join_v(join_h(join_v(m1, m3), join_v(m2, m4), m5), join_h(m5, m6)));

	BOOST_TEST((rt_id(m5) == id< std_matrix< int, 3_C, 3_R > >));
	BOOST_TEST((rt_id(m6) == id< std_matrix< int, 3_C, 3_R > >));
	BOOST_TEST((rt_id(m7) == id< std_matrix< int, 6_C, 6_R > >));

	BOOST_TEST((m5 == ref3x3));
	BOOST_TEST((m6 == ref3x3));
	BOOST_TEST((m7 == ref6x6));
}

BOOST_AUTO_TEST_CASE(test_join_rt){
	auto m1 = make_matrix(2_CD, 2_RD, {{0, 1}, {3, 4}});
	auto m2 = make_matrix(1_CD, 2_RD, {{2}, {5}});
	auto m3 = make_matrix(2_CD, 1_RD, {{6, 7}});
	auto m4 = make_matrix(1_CD, 1_RD, {{8}});

	auto m5 = join(join_v(
		join_h(m1, m2),
		join_h(m3, m4)
	));

	auto m6 = join(join_h(
		join_v(m1, m3),
		join_v(m2, m4)
	));

	auto m7 = join(
		join_v(join_h(join_v(m1, m3), join_v(m2, m4), m5), join_h(m5, m6)));

	BOOST_TEST((rt_id(m5) == id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST((rt_id(m6) == id< std_matrix< int, 0_C, 0_R > >));
	BOOST_TEST((rt_id(m7) == id< std_matrix< int, 0_C, 0_R > >));

	BOOST_TEST((m5 == ref3x3));
	BOOST_TEST((m6 == ref3x3));
	BOOST_TEST((m7 == ref6x6));
}

BOOST_AUTO_TEST_CASE(test_join_ct_and_rt_1){
	auto m1 = make_matrix(2_CD, 2_RD, {{0, 1}, {3, 4}});
	constexpr auto m2 = make_matrix(1_CS, 2_RS, {{2}, {5}});
	constexpr auto m3 = make_matrix(2_CS, 1_RS, {{6, 7}});
	constexpr auto m4 = make_matrix(1_CS, 1_RS, {{8}});

	auto m5 = join(join_v(
		join_h(m1, m2),
		join_h(m3, m4)
	));

	auto m6 = join(join_h(
		join_v(m1, m3),
		join_v(m2, m4)
	));

	auto m7 = join(
		join_v(join_h(join_v(m1, m3), join_v(m2, m4), m5), join_h(m5, m6)));

	BOOST_TEST((rt_id(m5) == id< std_matrix< int, 3_C, 3_R > >));
	BOOST_TEST((rt_id(m6) == id< std_matrix< int, 3_C, 3_R > >));
	BOOST_TEST((rt_id(m7) == id< std_matrix< int, 6_C, 6_R > >));

	BOOST_TEST((m5 == ref3x3));
	BOOST_TEST((m6 == ref3x3));
	BOOST_TEST((m7 == ref6x6));
}

BOOST_AUTO_TEST_CASE(test_join_ct_and_rt_2){
	auto m1 = make_matrix(2_CD, 2_RD, {{0, 1}, {3, 4}});
	constexpr auto m2 = make_matrix(1_CS, 2_RS, {{2}, {5}});
	constexpr auto m3 = make_matrix(2_CS, 1_RS, {{6, 7}});
	auto m4 = make_matrix(1_CD, 1_RD, {{8}});

	auto m5 = join(join_v(
		join_h(m1, m2),
		join_h(m3, m4)
	));

	auto m6 = join(join_h(
		join_v(m1, m3),
		join_v(m2, m4)
	));

	auto m7 = join(
		join_v(join_h(join_v(m1, m3), join_v(m2, m4), m5), join_h(m5, m6)));

	BOOST_TEST((rt_id(m5) == id< std_matrix< int, 0_C, 3_R > >));
	BOOST_TEST((rt_id(m6) == id< std_matrix< int, 3_C, 0_R > >));
	BOOST_TEST((rt_id(m7) == id< std_matrix< int, 0_C, 6_R > >));

	BOOST_TEST((m5 == ref3x3));
	BOOST_TEST((m6 == ref3x3));
	BOOST_TEST((m7 == ref6x6));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Oleg Maximenko 2014.
// 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)
//
// See http://github.com/svgpp/svgpp for library home page.

#pragma once

#include <cmath>
#include <boost/config.hpp>
#include <boost/math/constants/constants.hpp>

namespace svgpp { namespace policy
{

namespace marker_directionality
{

template<class Number = double>
struct radians
{
  typedef Number directionality_type;

  static BOOST_CONSTEXPR directionality_type undetermined_directionality() // "align with the positive x-axis in user space"
  {
    return 0;
  }

  template<class Coordinate>
  static directionality_type segment_directionality(Coordinate dx, Coordinate dy)
  {
    return std::atan2(dy, dx);
  }

  static directionality_type bisector_directionality(directionality_type in_segment, directionality_type out_segment)
  {
    directionality_type dir = (in_segment + out_segment) * 0.5;
    if (std::fabs(in_segment - out_segment) > boost::math::constants::pi<directionality_type>())
    {
      if (dir < 0)
        return dir + boost::math::constants::pi<directionality_type>();
      else
        return dir - boost::math::constants::pi<directionality_type>();
    }
    return dir;
  }
};

} // namespace marker_directionality

namespace markers
{

struct calculate
{
  typedef marker_directionality::radians<> directionality_policy;

  static const bool calculate_markers = true;
  static const bool always_calculate_auto_orient = false; // Doesn't call marker_get_config if true
};

struct calculate_always
{
  typedef marker_directionality::radians<> directionality_policy;

  static const bool calculate_markers = true;
  static const bool always_calculate_auto_orient = true; // Doesn't call marker_get_config if true
};

struct raw
{
  static const bool calculate_markers = false;
};

typedef raw default_policy;

template<class Context>
struct by_context
{
  typedef default_policy type;
};

}}}


#include "stdafx.h"

#include <boost/lexical_cast.hpp>
using boost::lexical_cast;

#include <boost/algorithm/clamp.hpp>
using boost::algorithm::clamp;

#include "bitmap.h"

// @param[in] inImage 入力画像
// @param[in] nMaxSearchRadius ディスタンス計算する時にデータを検索しに行く範囲を決める係数
// @param[in] outImage 出力先ディスタンスフィールド画像
void calc_distance_bmp(bitmap& inImage, float fMaxRadius, bitmap& outImage);

////////////////////////////
// main

// 元ファイル名 検索範囲係数 ディスタンスファイル名 ディスタンス横サイズ　ディスタンス縦サイズ
int main(int argc, char* argv[])
{
	string	sInFile;
	string	sOutFile;
	float   fRatio=2.0;
	int32_t	nWidth=0;
	int32_t	nHeight=0;

	if(argc!=6)
	{
		cout << "Usage:" << endl;
		cout << "\t元ファイル名 距離係数 ディスタンスファイル名 ディスタンス横サイズ ディスタンス縦サイズ" << endl;
		system("pause");
		return 1;
	}
	else
	{
		sInFile	 = argv[1];
		fRatio   = lexical_cast<float>(argv[2]);
		sOutFile = argv[3];
		nWidth	 = lexical_cast<int32_t>(argv[4]);
		nHeight	 = lexical_cast<int32_t>(argv[5]);

		cout << "arg:" << endl;
		cout << "\t" << sInFile.c_str() << " " << fRatio << " " << sOutFile.c_str() << " " << nWidth << " " << nHeight << endl;
	}

	bitmap inImage;
	if(!inImage.load(sInFile))
	{
		cout << sInFile.c_str() << "読み込み失敗しました。" << endl;
		system("pause");
		return 1;
	}

	// サイズの確認。ディスタンスファイルの方を大きくすることはできない
	if(inImage.info().biWidth < nWidth || inImage.info().biHeight < nHeight)
	{
		cout << "DistanceFieldの方を大きくすることはできません。" << endl;
		system("pause");
		return 1;
	}

	// ディスタンスファイルの準備
	bitmap outImage;
	outImage.set_info(nWidth, nHeight, 24);
	outImage.create_data();

	// ディスタンス計算＆プロット
	calc_distance_bmp(inImage, fRatio, outImage);

	// 書き出し
	outImage.write(sOutFile);

	system("pause");
	return 0;
}

//////////////////////////////////////////////
// distance計算系

// inImage内の(x,y)のディスタンスを計算する
float calc_distance_bmp_radius(bitmap& inImage, int32_t x, int32_t y, float fMaxRadius)
{
	uint32_t color = inImage.pixel(x,y);

	float minD  = fMaxRadius*fMaxRadius+1.0f/fMaxRadius;

	int32_t	sx,sy;
	int32_t	minX,maxX,minY,maxY;

	for(int32_t radius = 1; radius<fMaxRadius && radius*radius<minD; ++radius)
	{
		minX = clamp(x-radius, 0, inImage.info().biWidth-1);
		maxX = clamp(x+radius, 0, inImage.info().biWidth-1);
		minY = clamp(y-radius, 0, inImage.info().biHeight-1);
		maxY = clamp(y+radius, 0, inImage.info().biHeight-1);

		// ↑
		if(y+radius<inImage.info().biHeight)
		{
			for(sx = minX; sx<maxX; ++sx)
			{
				if(inImage.pixel(sx,maxY)!=color)
				{
					float d = static_cast<float>((sx-x)*(sx-x)+(maxY-y)*(maxY-y));
					if(d<minD) minD=d;
				}
			}
		}

		// ↓
		if(y-radius>0)
		{
			for(sx = minX; sx<maxX; ++sx)
			{
				if(inImage.pixel(sx,minY)!=color)
				{
					float d = static_cast<float>((sx-x)*(sx-x)+(minY-y)*(minY-y));
					if(d<minD) minD=d;
				}
			}
		}

		// →
		if(x+radius<inImage.info().biWidth)
		{
			for(sy = minY; sy<maxY; ++sy)
			{
				if(inImage.pixel(maxX,sy)!=color)
				{
					float d = static_cast<float>((maxX-x)*(maxX-x)+(sy-y)*(sy-y));
					if(d<minD) minD=d;
				}
			}
		}

		// ←
		if(x-radius>0)
		{
			for(sy = minY; sy<maxY; ++sy)
			{
				if(inImage.pixel(minX,sy)!=color)
				{
					float d = static_cast<float>((minX-x)*(minX-x)+(sy-y)*(sy-y));
					if(d<minD) minD=d;
				}
			}
		}
	}

	float df = sqrtf(minD);
	if(inImage.pixel(x,y)==0)
		df = -df;

	return df;
}

void calc_distance_bmp(bitmap& inImage, float fMaxRadiusRatio, bitmap& outImage)
{
	float fMaxRadius=1;

	if(inImage.info().biWidth > inImage.info().biHeight)
	{
		fMaxRadius = fMaxRadiusRatio*inImage.info().biWidth / outImage.info().biWidth;
	}
	else
	{
		fMaxRadius = fMaxRadiusRatio*inImage.info().biHeight / outImage.info().biHeight;
	}

	for(int32_t y=0; y<outImage.info().biHeight; ++y)
	{
		for(int32_t x=0; x<outImage.info().biWidth; ++x)
		{
			// ディスタンスフィールド上の位置
			uint32_t sx = x * (inImage.info().biWidth-1)  / (outImage.info().biWidth-1);
			uint32_t sy = y * (inImage.info().biHeight-1) / (outImage.info().biHeight-1);

			// 距離計算
			float d = calc_distance_bmp_radius(inImage,sx,sy,fMaxRadius);
			// 距離を比率にしてから、0～127.5にする
			d = 127.5f * d/fMaxRadius;
			d += 127.5f;
			uint32_t sd = clamp(static_cast<uint32_t>(d),0,255); // 端数繰り上げ

			// 計算結果を書き込む
			outImage.set_pixel(x,y,sd,sd,sd);
		}
	}
}


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>
#include <math.h>
#include <algorithm>
#include <time.h>
#include <fstream>
#include <deque>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#define PPT_SIZE 1000000000
#define MAX_STRING 100
#define MAX_NODE_NUM 1000000000
#define MAX_FEATURE_NUM 10000000	// warning: large MAX_FEATURE_NUM causes segmenation default
#define MAX_PARAMETER_NUM 300
#define PRIME_NUM 348513

using namespace std;

char networkFile[MAX_STRING], featureFile[MAX_STRING], embeddingFile[MAX_STRING], timeFile[MAX_STRING];

// return unsigned int* primeArray
void readRandomPrimes(string primesFile, unsigned int* primeArray) {
	string line;
	int i=0;
	ifstream  fin(primesFile);
	if(!fin.is_open()) {
		cout<<"primes file not found!"<<endl;
	}
	while(getline(fin, line) && i<PRIME_NUM) {
		primeArray[i] = stoi(line);
		i++;
	}
	fin.close();
}

void generateRandomPrimes(unsigned int* primeArray, int k, int iteration,
	unsigned int ***hashU1ParametersThreeArrays, unsigned int ***hashU2ParametersThreeArrays,
	unsigned int ***hashVParametersThreeArrays, unsigned int *divisor) {

	srand(1);
	int hashNum = k;
	for(int iR = 1; iR < iteration+1; iR++) {
		// coefficients
		unsigned int **hashU1ParametersTwoArrays = new unsigned int*[hashNum];
		unsigned int **hashU2ParametersTwoArrays = new unsigned int*[hashNum];
		unsigned int **hashVParametersTwoArrays = new unsigned int*[hashNum];

		for(int iK=0; iK < hashNum; iK++) {

			unsigned int *hashU1ParametersOneArray = new unsigned int[2];
			unsigned int *hashU2ParametersOneArray = new unsigned int[2];
			unsigned int *hashVParametersOneArray = new unsigned int[2];

			for(int iHash=0; iHash< 2; iHash++) {

				hashU1ParametersOneArray[iHash] = primeArray[rand()%PRIME_NUM];
				hashU2ParametersOneArray[iHash] = primeArray[rand()%PRIME_NUM];
				hashVParametersOneArray[iHash] = primeArray[rand()%PRIME_NUM];

			}

			hashU1ParametersTwoArrays[iK] = hashU1ParametersOneArray;
			hashU2ParametersTwoArrays[iK] = hashU2ParametersOneArray;
			hashVParametersTwoArrays[iK] = hashVParametersOneArray;

		}

		hashU1ParametersThreeArrays[iR] = hashU1ParametersTwoArrays;
		hashU2ParametersThreeArrays[iR] = hashU2ParametersTwoArrays;
		hashVParametersThreeArrays[iR] = hashVParametersTwoArrays;

	}

	divisor[0] = primeArray[PRIME_NUM-1];
}

// read network
// return int **network, int *adjNum
// index starts at 0
void readNetwork(char* adjListFile, int networkSize, int **network, int *adjNum) {
	string line;
	ifstream  fin(adjListFile);
	if(!fin.is_open()) {
		cout<<"adjacent list file not found!"<<endl;
	}
	vector<string> adjlist;
	int *nodes = NULL;
	for(int iRow=0; iRow< networkSize; iRow++) {
		getline(fin, line);
		if(line.empty()) {
			adjNum[iRow] = 0;
		} else {
			boost::split(adjlist, line, boost::is_any_of(" "), boost::token_compress_on);

			nodes = new int[adjlist.size()];
			for(int iNode=0; iNode < adjlist.size(); iNode++) {
				nodes[iNode] = stoi(adjlist.at(iNode));
			}
			network[iRow] = nodes;
			adjNum[iRow] = adjlist.size();
		}

		adjlist.clear();
	}
	fin.close();
}

// read feature
// features are seperated by one whitespace
// return unsigned int **tmpFeatureTwoArrays, int *tmpFeatureNumOneArray, int networkSize
int readFeatures(char* featureFile, unsigned int **tmpFeatureTwoArrays, int *tmpFeatureNumOneArray) {
	string line;
	ifstream  fin(featureFile);
	if(!fin.is_open()) {
		cout<<"feature file not found!"<<endl;
	}
	vector<string> featureVector;
	unsigned int *featureArray = NULL;
	int nodeNum = 0;
	while(getline(fin, line)) {

		if(line.empty()) {
			//featureArray = NULL;
			tmpFeatureNumOneArray[nodeNum] = 1;
			featureArray = new unsigned int[1];
			featureArray[0] = MAX_FEATURE_NUM;
		} else {
			boost::split(featureVector, line, boost::is_any_of(" "), boost::token_compress_on);
			featureArray = new unsigned int[featureVector.size()];
			for(int iFeature=0; iFeature < featureVector.size(); iFeature++) {
				featureArray[iFeature]= stoi(featureVector.at(iFeature));
			}
			tmpFeatureNumOneArray[nodeNum] = featureVector.size();
		}

		tmpFeatureTwoArrays[nodeNum] = featureArray;
		//tmpFeatureNumOneArray[nodeNum] = featureVector.size();
		nodeNum++;
		featureVector.clear();

	}
	fin.close();

	return nodeNum;
}

void minHash(unsigned int **hashParameters, unsigned int *divisor, int k, unsigned int *features, int featureNum, unsigned int* minHashValues, unsigned int* minHashResults){

	// row: feature
	// column: parameter
	// the operation of minhash is frequent, so static arrays are preferred for effeciency.

	if(features[0] == MAX_FEATURE_NUM) {
		for(int iColumn = 0; iColumn < k; iColumn++) {
			minHashValues[iColumn] = MAX_FEATURE_NUM;
			minHashResults[iColumn] = divisor[0] + 1;
		}
		return;
	}

	unsigned int **result =  new unsigned int *[featureNum];
	for(int iRow = 0; iRow< featureNum; iRow++) {
		result[iRow] = new unsigned int[k];
		for(int jColumn = 0; jColumn < k; jColumn++) {
			result[iRow][jColumn] = (features[iRow]* hashParameters[jColumn][0] + hashParameters[jColumn][1]) % divisor[0];
		}
	}

	unsigned int minValue;
	int minIndex;
	for(int iColumn = 0; iColumn < k; iColumn++) {
		minValue = result[0][iColumn];
		minIndex = 0;
		for(int jRow = 1; jRow < featureNum; jRow++) {
			if(minValue > result[jRow][iColumn]) {
				minValue = result[jRow][iColumn];
				minIndex = jRow;
			}
		}
		minHashValues[iColumn] = features[minIndex];
		minHashResults[iColumn] = minValue;
	}
	for (int iRow=0; iRow<featureNum; iRow++) {
        delete[] result[iRow];
    }
    delete[] result;
}

void mergeMessages(unsigned int *features, int featureNum, unsigned int *firstComponent, int firstComponentSize, unsigned int *mergedFeatures) {

	for(int iFeature = 0; iFeature < featureNum; iFeature++) {
		*mergedFeatures++ = *features++;
	}
	for(int iFeature = 0; iFeature < firstComponentSize; iFeature++) {
		*mergedFeatures++ = *firstComponent++;
	}
}

void passMessages(int **network, int *adjNum, int networkSize, int iteration, int k,
	unsigned int **featureTwoArrays, int *featureNumOneArray,
	unsigned int ***hashU1ParametersThreeArrays, unsigned int ***hashU2ParametersThreeArrays, unsigned int ***hashVParametersThreeArrays,
	unsigned int *divisor, unsigned int **fingerprints) {

	unsigned int *features = NULL;
	unsigned int *sumMessages = NULL;

	unsigned int *minHashValue = NULL;
	unsigned int *minHashResult = NULL;

	unsigned int *mergedFeatures = NULL;
	int mergedFeatureNum = 0;

	int featureNum=0;
	unsigned int *fingerprintResult = NULL;
	unsigned int *vMessage = NULL;
	unsigned int *vMinHashResult = NULL;
	unsigned int **vMessages = new unsigned int *[networkSize];
	//unsigned int **nodeFeatures = new unsigned int *[networkSize];
	for(int iNode=0; iNode<networkSize; iNode++) {

		fingerprints[iNode] = new unsigned int[k];
	}

	for(int iR=1; iR<iteration+1; iR++) {

		for(int iNode=0; iNode<networkSize; iNode++) {

			if(iR==1) {
				features = featureTwoArrays[iNode];
				featureNum = featureNumOneArray[iNode];
			} else {
				features = fingerprints[iNode];
				featureNum = k;
			}

			vMessage = new unsigned int[k];
			vMinHashResult = new unsigned int[k];
			minHash(hashVParametersThreeArrays[iR], divisor, k, features, featureNum, vMessage, vMinHashResult);
			vMessages[iNode] = vMessage;
			delete[] vMinHashResult;
		}

		for(int iNode=0; iNode<networkSize; iNode++) {

			if(iR==1) {
				features = featureTwoArrays[iNode];
				featureNum = featureNumOneArray[iNode];
			} else {
				features = fingerprints[iNode];
				featureNum = k;
			}

			sumMessages = new unsigned int[k*adjNum[iNode]];
			for(int iAdj=0; iAdj<adjNum[iNode]; iAdj++) {

				for(int iK=0; iK<k; iK++) {

					sumMessages[iAdj*k+iK] = vMessages[network[iNode][iAdj]][iK];
				}
			}

			mergedFeatureNum = featureNum + k*adjNum[iNode];
			mergedFeatures = new unsigned int [mergedFeatureNum];
			mergeMessages(features, featureNum, sumMessages, k*adjNum[iNode], mergedFeatures);
			fingerprintResult = new unsigned int[k];
			minHash(hashU1ParametersThreeArrays[iR], divisor, k, mergedFeatures, mergedFeatureNum, fingerprints[iNode], fingerprintResult);
			delete[] mergedFeatures;
			delete[] sumMessages;

		}

		for(int iNode=0; iNode<networkSize; iNode++) {

			delete[] vMessages[iNode];
		}
	}
	delete[] vMessages;
	/*
	for(int iNode=0; iNode<networkSize; iNode++) {

		fingerprints[iNode] = nodeFeatures[iNode];
	}*/

}

void output(unsigned int **fingerprints, int networkSize, int k, double elapsedTime) {
	ofstream fout(embeddingFile);
	if(!fout.is_open()) {
		cout<<"fail to open embedding file!"<<endl;
	}
	for(int iNetwork = 0; iNetwork< networkSize; iNetwork++) {
		for(int iK=0; iK< k; iK++) {
			fout<< fingerprints[iNetwork][iK]<< " ";
		}
		fout<<endl;
	}
	fout.close();
	ofstream fout1(timeFile);
	if(!fout1.is_open()) {
		cout<<"fail to open time file!"<<endl;
	}
	fout1<< elapsedTime<< endl;
	fout1.close();
}

int ArgPos(char *str, int argc, char **argv) {
	int a;
	for (a = 1; a < argc; a++) if (!strcmp(str, argv[a])) {
		if (a == argc - 1) {
			cout<<"Argument missing for "<< str<<endl;
			exit(1);
		}
		return a;
	}
	return -1;
}

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

	int k = 0;
	int iteration = 0;

	int i;
	if (argc == 1) {
		cout<<"Deep Recursive Hashing"<<endl;
		cout<<"Options:"<<endl;
		cout<<"Parameters for algorithm:"<<endl;
		cout<<"-network <file>"<<endl;
		cout<<"Network data of adjlist from <file> for structure"<<endl;
		cout<<"-feature <file>"<<endl;
		cout<<"Feature data from <file> for contents"<<endl;
		cout<<"-hashdim <int>"<<endl;
		cout<<"Number of dimension of network embeddings"<<endl;
		cout<<"-iteration <int>"<<endl;
		cout<<"iteration of neural network"<<endl;
		cout<<"-embedding <file>"<<endl;
		cout<<"<file> to save the embeddings"<<endl;
		cout<<"-time <file>"<<endl;
		cout<<"<file> to save running time"<<endl;
		return 0;
	}
	if ((i = ArgPos((char *)"-network", argc, argv)) > 0) {
		strcpy(networkFile, argv[i + 1]);
	}
	if ((i = ArgPos((char *)"-feature", argc, argv)) > 0) {
		strcpy(featureFile, argv[i + 1]);
	}
	if ((i = ArgPos((char *)"-hashdim", argc, argv)) > 0) {
		k = atoi(argv[i + 1]);
	}
	if ((i = ArgPos((char *)"-iteration", argc, argv)) > 0) {
		iteration = atoi(argv[i + 1]);
	}
	if ((i = ArgPos((char *)"-embedding", argc, argv)) > 0) {
		strcpy(embeddingFile, argv[i + 1]);
	}
	if ((i = ArgPos((char *)"-time", argc, argv)) > 0) {
		strcpy(timeFile, argv[i + 1]);
	}

	cout<< "hashdim: "<<k<< ", iteration: "<< iteration <<endl;

	// parameters for hash functions
	unsigned int *primeArray = new unsigned int[PRIME_NUM];
	unsigned int *divisor = new unsigned int[1];
	readRandomPrimes("primes.txt", primeArray);

	unsigned int ***hashU1ParametersThreeArrays = new unsigned int**[iteration+1];
	unsigned int ***hashU2ParametersThreeArrays = new unsigned int**[iteration+1];
	unsigned int ***hashVParametersThreeArrays = new unsigned int**[iteration+1];

	generateRandomPrimes(primeArray, k, iteration,
		hashU1ParametersThreeArrays, hashU2ParametersThreeArrays, hashVParametersThreeArrays, divisor);

	delete[] primeArray;

	// features on each node
	unsigned int **tmpFeatureTwoArrays = new unsigned int*[MAX_NODE_NUM];
	int *tmpFeatureNumOneArray = new int[MAX_NODE_NUM];
	int networkSize = readFeatures(featureFile, tmpFeatureTwoArrays, tmpFeatureNumOneArray);
	unsigned int **featureTwoArrays = new unsigned int*[networkSize];
	int *featureNumOneArray = new int[networkSize];
	for(int iNode = 0; iNode < networkSize; iNode++) {
		featureTwoArrays[iNode] = new unsigned int[tmpFeatureNumOneArray[iNode]];
		for(int iFeature = 0; iFeature < tmpFeatureNumOneArray[iNode]; iFeature++) {
			featureTwoArrays[iNode][iFeature] = tmpFeatureTwoArrays[iNode][iFeature];
		}
		featureNumOneArray[iNode] = tmpFeatureNumOneArray[iNode];
	}
	//delete temporary memory
	for(int iNode = 0; iNode < MAX_NODE_NUM; iNode++) {
		delete[] tmpFeatureTwoArrays[iNode];
	}
	delete[] tmpFeatureTwoArrays;
	delete[] tmpFeatureNumOneArray;

	// network structure
	int **network = new int*[networkSize];
	for(int iNode=0; iNode< networkSize; iNode++) {
		network[iNode] = NULL;
	}
	int *adjNum = new int[networkSize];
	readNetwork(networkFile, networkSize, network, adjNum);

	clock_t timeBegin, timeEnd;
	double elapsedTime;
	unsigned int **fingerprints = new unsigned int *[networkSize];

	timeBegin = clock();

	passMessages(network, adjNum, networkSize, iteration, k, featureTwoArrays, featureNumOneArray, hashU1ParametersThreeArrays, hashU2ParametersThreeArrays, hashVParametersThreeArrays, divisor, fingerprints);

	timeEnd = clock();
	elapsedTime = (double(timeEnd - timeBegin))/CLOCKS_PER_SEC;
	cout<< "elapsed time: "<< elapsedTime << endl;

	output(fingerprints, networkSize,  k, elapsedTime);

	return 0;

}


//
//  common.hpp
//  Elasticity
//
//  Created by Wim van Rees on 2/15/16.
//  Copyright © 2016 Wim van Rees. All rights reserved.
//

#ifndef common_hpp
#define common_hpp

#include <iostream>
#include <cassert>
#include <random>
#include <limits>
#include <utility>
#include <vector>
#include <string>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <map>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <array>
#include <set>
#include <memory>
#include <ctime>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/StdVector>

#include <igl/facet_components.h>

//#include "ArgumentParser.h"

typedef double Real;
typedef unsigned long long tUint;
typedef std::vector<Eigen::Matrix2d, Eigen::aligned_allocator<Eigen::Matrix2d>> tVecMat2d;
typedef std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> tVecMat3d;
enum MeshLayer {single, bottom, top};

//#define USETANTHETA

// little helper to avoid unused warnings when something is called in release mode and variable only checked in debug
#define _unused(x) ((void)(x))

namespace rnd
{
    /*! global random number generator */
    static std::mt19937 gen;
}

namespace helpers
{
    /*! convert an integer to a string with a specified number of zeros */
    inline std::string ToString(int value,int digitsCount)
    {
        std::ostringstream os;
        os<<std::setfill('0')<<std::setw(digitsCount)<<value;
        return os.str();
    }

    /*! convert an real to a string with a specified number of decimals */
    inline std::string ToStringPrecision(Real value, int digitsCount)
    {
        std::ostringstream out;
        out.precision(digitsCount);
        out << std::fixed << value;
        return out.str();
    }

    /*! check if two numbers are close (within some accuracy) */
    inline bool isclose(const Real val1, const Real val2, const bool printOnly=false, const Real rtol=1e-5, const Real atol=1e-8)
    {
        const Real diff = std::abs(val1-val2) ;
        bool retval = true;
        if(diff > (atol + rtol * std::abs(val2)))
            retval = false;
        if(diff > (atol + rtol * std::abs(val1)))
            retval = false;

        if(printOnly && not retval)
        {
            std::cout << "PROBLEM : " << val1 << "\t" << val2 << "\t" << diff << std::endl;
            retval = true;
        }

        return retval;
    }

    inline bool isPointInProjectedTriangle(const Real x, const Real y, const Eigen::Vector3d & v0, const Eigen::Vector3d & v1, const Eigen::Vector3d & v2)
    {
        // dont use the third dimension
        const Real xp[3] = {v0(0),v1(0),v2(0)};
        const Real yp[3] = {v0(1),v1(1),v2(1)};

        const int npoly = 3; // triangle

        int i,j,c=0;
        for (i = 0, j = npoly-1; i < npoly; j = i++) {
            if ((((yp[i] <= y) && (y < yp[j])) ||
                 ((yp[j] <= y) && (y < yp[i]))) &&
                (x < (xp[j] - xp[i]) * (y - yp[i]) / (yp[j] - yp[i]) + xp[i]))
                c = !c;
        }
        return c;
    }

    inline Eigen::Vector3d getBaryCentricWeights2D(const Eigen::Vector3d & v0, const Eigen::Vector3d & v1, const Eigen::Vector3d & v2, const Real x, const Real y)
    {
        assert(isPointInProjectedTriangle(x, y, v0, v1, v2));

        Eigen::Vector3d me;
        me << x, y, 0;
        // compute my point in barycentric coordinates of undeformed configuration
        // Compute barycentric coordinates (u, v, w) for
        // point p with respect to triangle (a, b, c)
        // http://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
        const Eigen::Vector3d tmp0 = v1 - v0;
        const Eigen::Vector3d tmp1 = v2 - v0;
        const Eigen::Vector3d tmp2 = me - v0;
        const Real d00 = tmp0.dot(tmp0);
        const Real d01 = tmp0.dot(tmp1);
        const Real d11 = tmp1.dot(tmp1);
        const Real d20 = tmp2.dot(tmp0);
        const Real d21 = tmp2.dot(tmp1);
        const Real denom = d00 * d11 - d01 * d01;
        const Real lambda1 = (d11 * d20 - d01 * d21) / denom;
        const Real lambda2 = (d00 * d21 - d01 * d20) / denom;
        const Real lambda0 = 1.0 - lambda1 - lambda2;
        // if me == v0 --> lambda0 = 1
        // if me == v1 --> lambda1 = 1
        // if me == v2 --> lambda2 = 1

        Eigen::Vector3d retval;
        retval << lambda0, lambda1, lambda2;
        return retval;
    }

    template<int component>
    inline Real getDisplacementOfPointInRestTriangle(const Eigen::Vector3d & rv0, const Eigen::Vector3d & rv1, const Eigen::Vector3d & rv2, const Eigen::Vector3d & v0, const Eigen::Vector3d & v1, const Eigen::Vector3d & v2, const Real x, const Real y)
    {

        const Eigen::Vector3d lambdas = getBaryCentricWeights2D(rv0, rv1, rv2, x, y);

        const Real displ_0 = (v0 - rv0)(component);
        const Real displ_1 = (v1 - rv1)(component);
        const Real displ_2 = (v2 - rv2)(component);

        // get my displacement
        return -(lambdas(0)*displ_0 + lambdas(1)*displ_1 + lambdas(2)*displ_2);
    }

    inline Real interpolateOverTriangle(const Eigen::Vector3d & v0, const Eigen::Vector3d & v1, const Eigen::Vector3d & v2, const Real x, const Real y)
    {
        // linear interpolation over the triangle
        const Real A = v0(1)*(v1(2)-v2(2)) + v1(1)*(v2(2)-v0(2)) + v2(1)*(v0(2)-v1(2));
        const Real B = v0(2)*(v1(0)-v2(0)) + v1(2)*(v2(0)-v0(0)) + v2(2)*(v0(0)-v1(0));
        const Real C = v0(0)*(v1(1)-v2(1)) + v1(0)*(v2(1)-v0(1)) + v2(0)*(v0(1)-v1(1));
        const Real D = v0(0)*(v1(1)*v2(2)-v1(2)*v2(1)) + v1(0)*(v2(1)*v0(2)-v2(2)*v0(1)) + v2(0)*(v0(1)*v1(2)-v0(2)*v1(1));

        const Real ptZ = (D-A*x-B*y)/C;

        return ptZ;
    }

    inline Real computeDistanceToLineSegment(const Eigen::Vector2d & l1, const Eigen::Vector2d & l2, const Eigen::Vector2d & pt)
    {
        //http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
        const Real Lsq = (l1-l2).squaredNorm();
        if(Lsq < std::numeric_limits<Real>::epsilon())
            return (l1-pt).norm();

        const Real t =  std::max(0.0, std::min(1.0, (pt-l1).dot(l2-l1) / Lsq));
        const Eigen::Vector2d projection = l1 + t * (l2 - l1);
        return (projection - pt).norm();
    }

    inline int getIntersectionPointBetweenVectors(const Eigen::Vector2d & pt1, const Eigen::Vector2d & n1, const Eigen::Vector2d & pt2, const Eigen::Vector2d & n2, Eigen::Vector2d & result)
    {
        // return 0 if intersection is found
        // return 1 if an intersection is found, but beyond the extent of pt1+n1 and pt2+n2
        // return 2 if no intersection is found (parallel vectors)

        const Eigen::Vector2d n1_hat = n1.normalized();
        const Eigen::Vector2d n2_hat = n2.normalized();

        const Real pt1_vec1 = pt1.dot(n1_hat);
        const Real pt1_vec2 = pt1.dot(n2_hat);
        const Real pt2_vec1 = pt2.dot(n1_hat);
        const Real pt2_vec2 = pt2.dot(n2_hat);
        const Real vec1_vec2 = n1_hat.dot(n2_hat);

        // deal with orthogonal
        if(std::abs(vec1_vec2 - 1) < std::numeric_limits<Real>::epsilon())
        {
            if((pt1-pt2).norm()  < std::numeric_limits<Real>::epsilon())
            {
                result = pt1;
                return 0;
            }
            else
            {
                return 2;
            }
        }

        const Real beta1 = (pt2_vec1 + (pt1_vec2 - pt2_vec2) * vec1_vec2 - pt1_vec1) / (1.0 - std::pow(vec1_vec2,2));
        const Real beta2 = (pt1_vec2 + (pt2_vec1 - pt1_vec1) * vec1_vec2 - pt2_vec2) / (1.0 - std::pow(vec1_vec2,2));

        //const Eigen::Vector2d retval1 = pt1 + beta1*n1_hat;
        const Eigen::Vector2d retval2 = pt2 + beta2*n2_hat;

        // set the intersection point
        result = retval2;

        const bool inRange_1 = (beta1 >= 0 && beta1 <= n1.norm());
        const bool inRange_2 = (beta2 >= 0 && beta2 <= n2.norm());
        return (inRange_1 && inRange_2) ? 0 : 1;
    }

    inline std::string removeExtension(const std::string input, const std::string extension)
    {
        // remove extension from input. Assume extension is given as (eg) .vtp (including the period)
        // if not found, return input unaltered
        const std::size_t found_ext = input.rfind(extension);
        const std::string output = (found_ext != std::string::npos ? input.substr(0, found_ext) : input);
        return output;
    }

    inline void catastrophe(std::string s, const char* file, const int line, const bool isFatal=true)    // write ``error: s and exit program
    {
        std::cerr << "Something went wrong, error: " << s << std::endl;
        std::cerr << "Called from file : " << file << " at line number " << line << std::endl;
        if(isFatal)
        {
            std::cout << "Exiting..." << std::endl;
            std::exit(1);
        }
    }

    template<class Matrix>
    inline void write_matrix_binary(const std::string & filename, const Matrix& matrix)
    {
        // from https://stackoverflow.com/a/25389481
        std::ofstream out(filename, std::ios::out | std::ios::binary | std::ios::trunc);
        typename Matrix::Index rows=matrix.rows(), cols=matrix.cols();
        out.write((char*) (&rows), sizeof(typename Matrix::Index));
        out.write((char*) (&cols), sizeof(typename Matrix::Index));
        out.write((char*) matrix.data(), rows*cols*sizeof(typename Matrix::Scalar) );
        out.close();
    }

    template<class Matrix>
    inline void read_matrix_binary(const std::string & filename, Matrix& matrix)
    {
        // from https://stackoverflow.com/a/25389481
        std::ifstream in(filename, std::ios::in | std::ios::binary);
        typename Matrix::Index rows=0, cols=0;
        in.read((char*) (&rows),sizeof(typename Matrix::Index));
        in.read((char*) (&cols),sizeof(typename Matrix::Index));
        matrix.resize(rows, cols);
        in.read( (char *) matrix.data() , rows*cols*sizeof(typename Matrix::Scalar) );
        in.close();
    }

    template<class Matrix>
    inline void write_vecmat_binary(const std::string & filename, const std::vector<Matrix, Eigen::aligned_allocator<Matrix>> & vecmat)
    {
        // from https://stackoverflow.com/a/25389481
        std::ofstream out(filename, std::ios::out | std::ios::binary | std::ios::trunc);
        const size_t vecsize = vecmat.size();
        if(vecsize==0) return;
        typename Matrix::Index rows=vecmat[0].rows(), cols=vecmat[0].cols();
        out.write((char*) &vecsize, sizeof(vecsize));
        out.write((char*) (&rows), sizeof(typename Matrix::Index));
        out.write((char*) (&cols), sizeof(typename Matrix::Index));
        out.write((char*)&vecmat[0], vecsize * rows*cols*sizeof(typename Matrix::Scalar));
        out.close();
    }

    template<class Matrix>
    inline void read_vecmat_binary(const std::string & filename, std::vector<Matrix, Eigen::aligned_allocator<Matrix>> & vecmat)
    {
        std::ifstream in(filename, std::ios::in | std::ios::binary);
        size_t vecsize = 0;
        typename Matrix::Index rows=0, cols=0;

        in.read((char*) (&vecsize),sizeof(vecsize));
        assert(vecsize > 0);

        in.read((char*) (&rows),sizeof(typename Matrix::Index));
        in.read((char*) (&cols),sizeof(typename Matrix::Index));

        assert(rows == Matrix::RowsAtCompileTime);
        assert(cols == Matrix::ColsAtCompileTime);

        vecmat.resize(vecsize);
        in.read( (char *) &vecmat[0] , vecsize * rows*cols*sizeof(typename Matrix::Scalar) );
        in.close();
    }
}

namespace Eigen
{
    /*! typedef vector of booleans */
    typedef Matrix<bool, 3, 1> Vector3b;

    /*! typedef vector of booleans */
    typedef Matrix<bool, Dynamic, 1> VectorXb;
    typedef Matrix<bool, Dynamic, Dynamic> MatrixXb;
}

inline Eigen::MatrixXd getDataFromFile(const std::string filename, const int nColumns)
{
    std::ifstream file(filename);
    double temp;
    std::vector<Real> temp_values;
    if (file.is_open()) {
        while (true) {
            file >> temp;
            temp_values.push_back(temp);
            if (file.eof()) { break; }
        }
        file.close();
    }
    else {
        std::cout << "Error reading file '" << filename << "'." << std::endl;
        exit(1);
    }

    Eigen::MatrixXd data(temp_values.size() / nColumns, nColumns);
    for (int i = 0; i < data.rows(); i++) {
        for (int j = 0; j < data.cols(); j++) {
            data(i, j) = temp_values[i * data.cols() + j];
        }
    }

    return data;
}

inline bool pointInsidePolygon(const Eigen::MatrixXd & polygonVertices, const Eigen::VectorXd & point)
{
    // Polygon must be defined in an oriented fashion. See https://wrf.ecse.rpi.edu/Research/Short_Notes/pnpoly.html
    int j = polygonVertices.rows() - 1;
    bool inside = false;
    bool flippedYet = false;
    for (int i = 0; i < polygonVertices.rows(); i++) {
        if ((polygonVertices(i, 1) > point(1)) != (polygonVertices(j, 1) > point(1))) {
            Real x = (polygonVertices(j, 0) - polygonVertices(i, 0)) * (point(1) - polygonVertices(i, 1)) / (polygonVertices(j, 1) - polygonVertices(i, 1)) + polygonVertices(i, 0);
            if (point(0) < x) {
                inside = !inside;
            }
        }
        j = i;
    }
    return inside;
}

inline void removePolygonFromMesh(Eigen::MatrixXd & vertices, Eigen::MatrixXi & faces, Eigen::MatrixXb & vertices_bc, const Eigen::MatrixXd & polygonVertices)
{
    // Remove the part of a mesh that lies within some polygon
    int nKeepFaces = 0;
    Eigen::VectorXi trianglesPerVertex = Eigen::VectorXi::Constant(vertices.rows(), 0);
    int nKeepVertices = 0;
    Eigen::VectorXi vertexMap(vertices.rows());

    // Only keep faces that don't lie within the polygon
    for (int i = 0; i < faces.rows(); i++) {
        Eigen::Vector2d faceCenter((vertices(faces(i, 0), 0) + vertices(faces(i, 1), 0) + vertices(faces(i, 2), 0)) / 3.0,
                                   (vertices(faces(i, 0), 1) + vertices(faces(i, 1), 1) + vertices(faces(i, 2), 1)) / 3.0);
        if (!pointInsidePolygon(polygonVertices, faceCenter)) {
            for (int j = 0; j < 3; j++) { trianglesPerVertex(faces(i, j)) += 1; }
            faces.row(nKeepFaces) = faces.row(i);
            nKeepFaces++;
        }
    }
    faces.conservativeResize(nKeepFaces, faces.cols());

    // Only keep vertices (and their boundary conditions) that are still referenced by a triangle
    for (int i = 0; i < vertices.rows(); i++) {
        if (trianglesPerVertex(i) > 0) {
            vertices.row(nKeepVertices) = vertices.row(i);
            vertices_bc.row(nKeepVertices) = vertices_bc.row(i);
            vertexMap(i) = nKeepVertices; // new index
            nKeepVertices++;
        }
        else {
            vertexMap(i) = -1;
        }
    }
    vertices.conservativeResize(nKeepVertices, vertices.cols());
    vertices_bc.conservativeResize(nKeepVertices, vertices_bc.cols());

    // Update vertex indices referenced by the faces
    for (int i = 0; i < faces.rows(); i++) {
        for (int j = 0; j < 3; j++) {
            if (vertexMap(faces(i, j)) < 0) {
                std::cout << "Error: old vertex " << faces(i, j) << " was indexed by an updated face!" << std::endl;
                exit(1);
            }
            faces(i, j) = vertexMap(faces(i, j));
        }
    }
}

inline void removeUnconnectedComponents(Eigen::MatrixXd & vertices, Eigen::MatrixXi & faces, Eigen::MatrixXb & vertices_bc, int vIndex = 0)
{
    Eigen::VectorXi F1;
    igl::facet_components(faces, F1);
    int fComponent = 0;
    for (int i = 0; i < faces.rows(); i++) {
        if (faces(i, 0) == vIndex || faces(i, 1) == vIndex || faces(i, 2) == vIndex) {
            fComponent = F1(i);
            break;
        }
    }

    // Only keep faces that correspond to this component
    int nKeepFaces = 0;
    Eigen::VectorXi trianglesPerVertex = Eigen::VectorXi::Constant(vertices.rows(), 0);
    for (int i = 0; i < faces.rows(); i++) {
        if (F1(i) == fComponent) {
            for (int j = 0; j < 3; j++) { trianglesPerVertex(faces(i, j)) += 1; }
            faces.row(nKeepFaces) = faces.row(i);
            nKeepFaces++;
        }
    }
    faces.conservativeResize(nKeepFaces, faces.cols());

    // Only keep vertices (and their boundary conditions) that are still referenced by a triangle
    int nKeepVertices = 0;
    Eigen::VectorXi vertexMap(vertices.rows());
    for (int i = 0; i < vertices.rows(); i++) {
        if (trianglesPerVertex(i) > 0) {
            vertices.row(nKeepVertices) = vertices.row(i);
            vertices_bc.row(nKeepVertices) = vertices_bc.row(i);
            vertexMap(i) = nKeepVertices; // new index
            nKeepVertices++;
        }
        else {
            vertexMap(i) = -1;
        }
    }
    vertices.conservativeResize(nKeepVertices, vertices.cols());
    vertices_bc.conservativeResize(nKeepVertices, vertices_bc.cols());

    // Update vertex indices referenced by the faces
    for (int i = 0; i < faces.rows(); i++) {
        for (int j = 0; j < 3; j++) {
            if (vertexMap(faces(i, j)) < 0) {
                std::cout << "Error: old vertex " << faces(i, j) << " was indexed by an updated face!" << std::endl;
                exit(1);
            }
            faces(i, j) = vertexMap(faces(i, j));
        }
    }
}

#endif /* common_hpp */


#define BOOST_TEST_MODULE pcraster geo neighbour
#include <boost/test/unit_test.hpp>
#include "geo_rasterdim.h"
#include "geo_neighbour.h"

static bool neighbourTarget(
    geo::LinearLoc  s,
    const geo::RasterDim&  rd,
    geo::LDD::Code  l,
    bool       outside)
{
  using namespace geo;

  LinearLoc d=LDD::target(s,l,rd.nrCells(),rd.nrCols());
  if (d>=rd.nrCells())
    return outside;
  CellLoc d2= LDD::target(rd.convert(s),l);
  return d2 == rd.convert(d);
}

static bool nbTarget(
    geo::LinearLoc  s,
    const geo::RasterDim&  rd,
    geo::NB::Code    l,
    bool       outside)
{
  using namespace geo;

  LinearLoc d=NB::target(s,l,rd.nrCells(),rd.nrCols() );
  if (d>=rd.nrCells())
    return outside;
  CellLoc d2= NB::target(rd.convert(s),l);
  return d2 == rd.convert(d);
}

BOOST_AUTO_TEST_CASE(linear_down_stream)
{
  using namespace geo;

  RasterDim rd(5,3);

  // all NBS are inside grid
  LinearLoc f=rd.convert(CellLoc(1,1));

  BOOST_CHECK(neighbourTarget(f,rd,1, false));
  BOOST_CHECK(neighbourTarget(f,rd,2, false));
  BOOST_CHECK(neighbourTarget(f,rd,3, false));
  BOOST_CHECK(neighbourTarget(f,rd,4, false));
  BOOST_CHECK(neighbourTarget(f,rd,5, false));
  BOOST_CHECK(neighbourTarget(f,rd,6, false));
  BOOST_CHECK(neighbourTarget(f,rd,7, false));
  BOOST_CHECK(neighbourTarget(f,rd,8, false));
  BOOST_CHECK(neighbourTarget(f,rd,9, false));

  BOOST_CHECK(nbTarget(f,rd,0, false));
  BOOST_CHECK(nbTarget(f,rd,1, false));
  BOOST_CHECK(nbTarget(f,rd,2, false));
  BOOST_CHECK(nbTarget(f,rd,3, false));
  BOOST_CHECK(nbTarget(f,rd,4, false));
  BOOST_CHECK(nbTarget(f,rd,5, false));
  BOOST_CHECK(nbTarget(f,rd,6, false));
  BOOST_CHECK(nbTarget(f,rd,7, false));

  // left NBs are outside
  f=rd.convert(CellLoc(1,0));

  BOOST_CHECK(neighbourTarget(f,rd,1, true ));
  BOOST_CHECK(neighbourTarget(f,rd,2, false));
  BOOST_CHECK(neighbourTarget(f,rd,3, false));
  BOOST_CHECK(neighbourTarget(f,rd,4, true ));
  BOOST_CHECK(neighbourTarget(f,rd,5, false));
  BOOST_CHECK(neighbourTarget(f,rd,6, false));
  BOOST_CHECK(neighbourTarget(f,rd,7, true ));
  BOOST_CHECK(neighbourTarget(f,rd,8, false));
  BOOST_CHECK(neighbourTarget(f,rd,9, false));

  // top/left NBs are outside
  f=rd.convert(CellLoc(1,0));

  BOOST_CHECK(neighbourTarget(f,rd,1, true ));
  BOOST_CHECK(neighbourTarget(f,rd,2, false));
  BOOST_CHECK(neighbourTarget(f,rd,3, false));
  BOOST_CHECK(neighbourTarget(f,rd,4, true ));
  BOOST_CHECK(neighbourTarget(f,rd,5, false));
  BOOST_CHECK(neighbourTarget(f,rd,6, false));
  BOOST_CHECK(neighbourTarget(f,rd,7, true ));
  BOOST_CHECK(neighbourTarget(f,rd,8, true ));
  BOOST_CHECK(neighbourTarget(f,rd,9, true ));

  // bottom/right NBs are outside
  f=rd.convert(CellLoc(rd.nrRows()-1,rd.nrCols()-1));

  BOOST_CHECK(neighbourTarget(f,rd,1, true ));
  BOOST_CHECK(neighbourTarget(f,rd,2, true ));
  BOOST_CHECK(neighbourTarget(f,rd,3, true ));
  BOOST_CHECK(neighbourTarget(f,rd,4, false));
  BOOST_CHECK(neighbourTarget(f,rd,5, false));
  BOOST_CHECK(neighbourTarget(f,rd,6, true ));
  BOOST_CHECK(neighbourTarget(f,rd,7, false));
  BOOST_CHECK(neighbourTarget(f,rd,8, false));
  BOOST_CHECK(neighbourTarget(f,rd,9, true ));
}

BOOST_AUTO_TEST_CASE(conversion)
{
  using namespace geo;

  BOOST_CHECK(LDD::toNB(1)==0);
  BOOST_CHECK(LDD::toNB(9)==7);
  BOOST_CHECK(NB::toLDD(0)==1);
  BOOST_CHECK(NB::toLDD(7)==9);
  // test Koenig lookup trick
  {
   NB::Code c=7;
   BOOST_CHECK(NB::toLDD(c)==9);
   BOOST_CHECK(NB::reverse(c)==0);
  }
}

BOOST_AUTO_TEST_CASE(nb_code)
{
  using namespace geo;

/* A one byte Neighbour bit code can be used to encode multiple neighbours.
 * Bit nrs set refer to the following ldd:
 *  5   6   7
 *   \  |  /
 *  3 - * - 4
 *    / |  \
 *  0   1   2
 *  AKA as the NBCode
 */
  CellLoc  from(100,50);
  BOOST_CHECK(NB::code(from,CellLoc(101,49))==0);
  BOOST_CHECK(NB::code(from,CellLoc(101,50))==1);
  BOOST_CHECK(NB::code(from,CellLoc(101,51))==2);
  BOOST_CHECK(NB::code(from,CellLoc(100,49))==3);
  BOOST_CHECK(NB::code(from,CellLoc(100,51))==4);
  BOOST_CHECK(NB::code(from,CellLoc( 99,49))==5);
  BOOST_CHECK(NB::code(from,CellLoc( 99,50))==6);
  BOOST_CHECK(NB::code(from,CellLoc( 99,51))==7);

  BOOST_CHECK( NB::diagonal(from,CellLoc(101,49)));
  BOOST_CHECK(!NB::diagonal(from,CellLoc(101,50)));
  BOOST_CHECK( NB::diagonal(from,CellLoc(101,51)));
  BOOST_CHECK(!NB::diagonal(from,CellLoc(100,49)));
  BOOST_CHECK(!NB::diagonal(from,CellLoc(100,51)));
  BOOST_CHECK( NB::diagonal(from,CellLoc( 99,49)));
  BOOST_CHECK(!NB::diagonal(from,CellLoc( 99,50)));
  BOOST_CHECK( NB::diagonal(from,CellLoc( 99,51)));
}

BOOST_AUTO_TEST_CASE(down_stream_visitor_cell)
{
  using namespace geo;

  /*! DownStreamVisitorCell uses bitfields for storage efficiency
   *  what is tested below is that the two bitfield members together
   *  fit in 4 bytes.
   */
   if (sizeof(size_t) == 4) {
    BOOST_CHECK(sizeof(geo::DownStreamVisitorCell) <= (sizeof(geo::CellLoc)+4));
   } else {
    // Bugzilla #133
    bool todoBitPackingOnNon32Bit=
      sizeof(geo::DownStreamVisitorCell) <= (sizeof(geo::CellLoc)+4);
    BOOST_WARN(todoBitPackingOnNon32Bit);
   }
}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <vector>
#include <string>
#include <map>

#include <boost/lexical_cast.hpp>

#include "main.hpp"

#include <hashclash/timer.hpp>
#include <hashclash/saveload_gz.hpp>
#include <hashclash/md5detail.hpp>
#include <hashclash/differentialpath.hpp>
#include <hashclash/booleanfunction.hpp>

using namespace hashclash;
using namespace std;

booleanfunction* md5bf(unsigned t)
{
	if (t < 16) return &MD5_F_data;
	else if (t < 32) return &MD5_G_data;
	else if (t < 48) return &MD5_H_data;
	return &MD5_I_data;
}

//unsigned t;
//booleanfunction* Ft;
//booleanfunction* Ftp1;
//booleanfunction* Ftp2;
//booleanfunction* Ftp3;
//vector<uint32> dFt, dFtp1, dFtp2, dFtp3;
//uint32 dmt, dmtp1, dmtp2, dmtp3;
//uint32 dQtp1, dQtp2, dQtp3, dQtp4;
//differentialpath newpath;

//bool lastdFp1, lastdFp2, lastdFp3;
void md5_connect_thread::connectbits(const connect_bitdata& in, vector<connect_bitdata>& out,
				 unsigned b, const differentialpath& lower, const differentialpath& upper,
				 vector<byteconditions>* newconds/* = 0*/
				 )
{
//	static connect_bitdata result;
//	static bf_outcome bfo0, bfo1, bfo2, bfo3;
//	static bf_conditions bfc0, bfc1, bfc2, bfc3;
//	static bitcondition Qt, Qtp1;
//	static byteconditions newcond;
	bitcondition Qtp2 = upper[t+2][b];
	bitcondition Qtp3 = upper[t+3][b];
	bitcondition Qtm1 = lower[t-1][b];
	bitcondition Qtm2 = lower[t-2][b];
	unsigned dQtbit = (in.dQt>>b)&1;
	unsigned dQtp1bit = (in.dQtp1>>b)&1;

	/**** carry dQt ****/
	for (unsigned dqt = 0; dqt <= dQtbit; ++dqt)
	{
		if (dqt == 0) {
			if (dQtbit == 0) {
				Qt = bc_constant;
				result.dQt = in.dQt;
			} else {
				Qt = bc_minus;
				result.dQt = in.dQt + (1<<b);
			}
		} else {
			Qt = bc_plus;
			result.dQt = in.dQt - (1<<b);
		}

		/**** bitconditions Ft ****/
		bfo0 = Ft->outcome(Qt, Qtm1, Qtm2);
		for (unsigned bfo0index = 0; bfo0index < bfo0.size(); ++bfo0index)
		{
			result.dFt = in.dFt - bfo0(bfo0index, b);
			if (result.dFt & (1<<b)) continue;
			bfc0 = Ft->forwardconditions(Qt, Qtm1, Qtm2, bfo0[bfo0index]);

			/**** carry dQtp1 ****/
			for (unsigned dqtp1 = 0; dqtp1 <= dQtp1bit; ++dqtp1)
			{
				if (dqtp1 == 0) {
					if (dQtp1bit == 0) {
						Qtp1 = bc_constant;
						result.dQtp1 = in.dQtp1;
					} else {
						Qtp1 = bc_minus;
						result.dQtp1 = in.dQtp1 + (1<<b);
					}
				} else {
					Qtp1 = bc_plus;
					result.dQtp1 = in.dQtp1 - (1<<b);
				}

				/**** bitconditions Ftp1 ****/
				bfo1 = Ftp1->outcome(Qtp1, bfc0.first, bfc0.second);
				for (unsigned bfo1index = 0; bfo1index < bfo1.size(); ++bfo1index)
				{
					result.dFtp1 = in.dFtp1 - bfo1(bfo1index, b);
					if (result.dFtp1 & (1<<b)) continue;
					bfc1 = Ftp1->forwardconditions(Qtp1, bfc0.first, bfc0.second, bfo1[bfo1index]);

					/**** bitconditions Ftp2 ****/
					bfo2 = Ftp2->outcome(Qtp2, bfc1.first, bfc1.second);
					for (unsigned bfo2index = 0; bfo2index < bfo2.size(); ++bfo2index)
					{
						result.dFtp2 = in.dFtp2 - bfo2(bfo2index, b);
						if (result.dFtp2 & (1<<b)) continue;
						bfc2 = Ftp2->forwardconditions(Qtp2, bfc1.first, bfc1.second, bfo2[bfo2index]);

						/**** bitconditions Ftp3 ****/
						bfo3 = Ftp3->outcome(Qtp3, bfc2.first, bfc2.second);
						for (unsigned bfo3index = 0; bfo3index < bfo3.size(); ++bfo3index)
						{
							result.dFtp3 = in.dFtp3 - bfo3(bfo3index, b);
							if (result.dFtp3 & (1<<b)) continue;

							out.push_back(result);
							if (newconds)
							{
								bfc3 = Ftp3->forwardconditions(Qtp3, bfc2.first, bfc2.second, bfo3[bfo3index]);
								newcond.set(
									bfc0.third, bfc1.third, bfc2.third,
									bfc3.third, bfc3.second, bfc3.first);
								newconds->push_back(newcond);
							}
						} // dFtp3
					} // dFtp2
				} // dFtp1
			} // dQtp1
		} // dFt
	} // dQt
}

void md5_connect_thread::connectbits2(const connect_bitdata& in,
				 unsigned b, const differentialpath& lower, const differentialpath& upper)
{
//	static bf_outcome bfo0, bfo1, bfo2, bfo3;
//	static bf_conditions bfc0, bfc1, bfc2, bfc3;
	bitcondition Qt, Qtp1;
	bitcondition Qtp2 = upper[t+2][b];
	bitcondition Qtp3 = upper[t+3][b];
	bitcondition Qtm1 = lower[t-1][b];
	bitcondition Qtm2 = lower[t-2][b];
	unsigned dQtbit = (in.dQt>>b)&1;
	unsigned dQtp1bit = (in.dQtp1>>b)&1;
	/**** carry dQt ****/
	for (unsigned dqt = 0; dqt <= dQtbit; ++dqt)
	{
		if (dqt == 0) {
			if (dQtbit == 0) {
				Qt = bc_constant;
			} else {
				Qt = bc_minus;
			}
		} else {
			Qt = bc_plus;
		}

		/**** bitconditions Ft ****/
		bfo0 = Ft->outcome(Qt, Qtm1, Qtm2);
		for (unsigned bfo0index = 0; bfo0index < bfo0.size(); ++bfo0index)
		{
			if ((in.dFt - bfo0(bfo0index, b)) & (1<<b)) continue;
			bfc0 = Ft->forwardconditions(Qt, Qtm1, Qtm2, bfo0[bfo0index]);

			/**** carry dQtp1 ****/
			lastdFp1 = true;
			for (unsigned dqtp1 = 0; dqtp1 <= dQtp1bit; ++dqtp1)
			{
				if (dqtp1 == 0) {
					if (dQtp1bit == 0) {
						Qtp1 = bc_constant;
					} else {
						Qtp1 = bc_minus;
					}
				} else {
					Qtp1 = bc_plus;
				}

				/**** bitconditions Ftp1 ****/
				bfo1 = Ftp1->outcome(Qtp1, bfc0.first, bfc0.second);
				for (unsigned bfo1index = 0; bfo1index < bfo1.size(); ++bfo1index)
				{
					if ((in.dFtp1 - bfo1(bfo1index, b)) & (1<<b)) continue;
					bfc1 = Ftp1->forwardconditions(Qtp1, bfc0.first, bfc0.second, bfo1[bfo1index]);

					/**** bitconditions Ftp2 ****/
					lastdFp2 = true;
					bfo2 = Ftp2->outcome(Qtp2, bfc1.first, bfc1.second);
					for (unsigned bfo2index = 0; bfo2index < bfo2.size(); ++bfo2index)
					{
						if ((in.dFtp2 - bfo2(bfo2index, b)) & (1<<b)) continue;

						lastdFp3 = true;
					} // dFtp2
				} // dFtp1
			} // dQtp1
		} // dFt
	} // dQt
}

unsigned md5_connect_thread::md5_connect_bits(const vector<differentialpath>& lowers, unsigned index,
						  const differentialpath& upper, path_container& container)
{
	const differentialpath& lower = lowers[index];
//	static connect_bitdata startbit0;
//	static vector<connect_bitdata> bitdataresults[33];

	// first verify whether lower and upper paths can be connected
	startbit0.dFt = dFt[index];
	startbit0.dFtp1 = dFtp1[index];
	startbit0.dFtp2 = dFtp2[index];
	startbit0.dFtp3 = dFtp3[index];
	startbit0.dQt = lowdQt[index]; //lower[t].diff();
	startbit0.dQtp1 = dQtp1; // upper[t+1].diff();

	bitdataresults[0].clear();
	bitdataresults[0].push_back(startbit0);

	for (unsigned b = 0; b < 32; ++b)
	{
		bitdataresults[b+1].clear();
		for (unsigned k = 0; k < bitdataresults[b].size(); ++k)
			connectbits(bitdataresults[b][k], bitdataresults[b+1], b, lower, upper);
		if (bitdataresults[b+1].size() == 0)
		{
			// they cannot be connected
			// check whether dF[t+1][b], dF[t+2][b] or dF[t+3][b] were involved or not
			lastdFp1 = false;
			lastdFp2 = false;
			lastdFp3 = false;
			for (unsigned k = 0; k < bitdataresults[b].size(); ++k)
				connectbits2(bitdataresults[b][k], b, lower, upper);
			return b;
		}

		// remove duplicates
		sort(bitdataresults[b+1].begin(), bitdataresults[b+1].end());
		bitdataresults[b+1].erase( unique(bitdataresults[b+1].begin(), bitdataresults[b+1].end())
									, bitdataresults[b+1].end());
	}
	// they can be connected
	// we rebuild the list and now keeping track
	// which bitconds lead which connect_bitdata to which connect_bitdata
//	static vector<connect_bitdata> bitdatastart[32];
//	static vector<connect_bitdata> bitdataend[32];
//	static vector<byteconditions>  bitdatanewcond[32];
	for (unsigned b = 0; b < 32; ++b)
	{
		bitdatastart[b].clear();
		bitdataend[b].clear();
		bitdatanewcond[b].clear();

		for (unsigned k = 0; k < bitdataresults[b].size(); ++k)
		{
			connectbits(bitdataresults[b][k], bitdataend[b], b, lower, upper, &bitdatanewcond[b]);
			bitdatastart[b].resize(bitdataend[b].size(), bitdataresults[b][k]);
		}
		if (bitdatastart[b].size() != bitdataend[b].size()
			|| bitdataend[b].size() != bitdatanewcond[b].size())
			throw;
	}

	//static differentialpath newpath2;
	newpath2 = upper;
	for (int k = lower.tbegin(); k < lower.tend(); ++k)
		newpath2[k] = lower[k];

#if 1
	vector< vector<unsigned> > tunnelstrength(32);
	vector< vector<unsigned> > colbitconditions(32);
	vector< vector<unsigned> > cumtunnelstrength, cumbitconditions;
	unsigned maxtunnelstrength = 0, minconditions = 9999;
	if (container.Qcondstart > t+3) {
		if (tmppath.path.size() == 0) {
			tmppath.get(-3); tmppath.get(16);
			for (int i = -3; i <= 16; ++i)
				for (unsigned b = 0; b < 32; ++b)
					tmppath.setbitcondition(i,b,bc_one);
		}
		for (unsigned b = 0; b < 32; ++b) {
			for (int i = newpath2.tbegin(); i < newpath2.tend(); ++i)
				tmppath.setbitcondition(i, b, newpath2(i,b));
			unsigned bc = 0;
			for (int i = newpath2.tbegin(); i < newpath2.tend(); ++i)
				if ((i < t-2 || i > t+3) && newpath2(i,b) != bc_constant)
					++bc;
			for (unsigned i = 0; i < bitdatanewcond[b].size(); ++i) {
				tmppath.setbitcondition(t-2,b,bitdatanewcond[b][i][0]);
				tmppath.setbitcondition(t-1,b,bitdatanewcond[b][i][1]);
				tmppath.setbitcondition(t,b,bitdatanewcond[b][i][2]);
				tmppath.setbitcondition(t+1,b,bitdatanewcond[b][i][3]);
				tmppath.setbitcondition(t+2,b,bitdatanewcond[b][i][4]);
				tmppath.setbitcondition(t+3,b,bitdatanewcond[b][i][5]);
				tunnelstrength[b].push_back(totaltunnelstrength(tmppath));
				unsigned bcc = bc;
				if (bitdatanewcond[b][i][0] != bc_constant) ++bcc;
				if (bitdatanewcond[b][i][1] != bc_constant) ++bcc;
				if (bitdatanewcond[b][i][2] != bc_constant) ++bcc;
				if (bitdatanewcond[b][i][3] != bc_constant) ++bcc;
				if (bitdatanewcond[b][i][4] != bc_constant) ++bcc;
				if (bitdatanewcond[b][i][5] != bc_constant) ++bcc;
				colbitconditions[b].push_back(bcc);
			}
			for (int i = newpath.tbegin(); i < newpath.tend(); ++i)
				tmppath.setbitcondition(i, b, bc_one);
		}
		cumtunnelstrength = tunnelstrength;
		cumbitconditions = colbitconditions;
		for (int b = 0; b < 31; ++b) {
			for (unsigned i = 0; i < bitdatanewcond[b].size(); ++i) {
				unsigned cts = cumtunnelstrength[b][i];
				pair< vector<connect_bitdata>::const_iterator, vector<connect_bitdata>::const_iterator >
					bounds = equal_range(bitdatastart[b+1].begin(), bitdatastart[b+1].end(), bitdataend[b][i]);
				for (; bounds.first != bounds.second; ++bounds.first) {
					unsigned j = bounds.first - bitdatastart[b+1].begin();
					if (tunnelstrength[b+1][j] + cts >= cumtunnelstrength[b+1][j]) {
						if (tunnelstrength[b+1][j] + cts > cumtunnelstrength[b+1][j] || colbitconditions[b+1][j] + cumbitconditions[b][i] < cumbitconditions[b+1][j])
							cumbitconditions[b+1][j] = colbitconditions[b+1][j] + cumbitconditions[b][i];
						cumtunnelstrength[b+1][j] = tunnelstrength[b+1][j] + cts;
					}
				}
			}
		}
		for (unsigned i = 0; i < cumtunnelstrength[31].size(); ++i)
			if (cumtunnelstrength[31][i] >= maxtunnelstrength) {
				if (cumtunnelstrength[31][i] > maxtunnelstrength || cumbitconditions[31][i] < minconditions)
					minconditions = cumbitconditions[31][i];
				maxtunnelstrength = cumtunnelstrength[31][i];
			}
		int maxcompl = maxtunnelstrength;
		for (int k = newpath2.tbegin(); k < newpath2.tend() && k < 64; ++k)
			if (k >= container.Qcondstart)
				maxcompl -= int(newpath2[k].hw());
		if (maxcompl < container.bestmaxcomp) return 32;
		if (maxtunnelstrength < container.bestmaxtunnel) return 32;
		if (minconditions > container.bestpathcond) return 32;
	}
#endif

	//static unsigned bindex[32];
	unsigned bit = 31;
	bindex[bit] = 0;
	vector<unsigned> tsv(33,0), bcv(33,0);
	tsv[bit] = tunnelstrength[31][0];
	bcv[bit] = colbitconditions[31][0];
	while (bit <= 31)
	{
		if (bindex[bit] < bitdataend[bit].size())
		{
#if 1
			unsigned ts = tsv[bit] = tsv[bit+1] + tunnelstrength[bit][bindex[bit]];
			unsigned bc = bcv[bit] = bcv[bit+1] + colbitconditions[bit][bindex[bit]];
			// ts == tsv[bit+1] after --bit;
#endif
			--bit;
			if (bit != 0) {
				bindex[bit] = bitdataend[bit].size();
				for (unsigned k = 0; k < bitdataend[bit].size(); ++k)
					if (bitdataend[bit][k] == bitdatastart[bit+1][bindex[bit+1]])
					{
#if 1
						if (cumtunnelstrength[bit][k] + ts < container.bestmaxtunnel) continue;
						if (cumtunnelstrength[bit][k] + ts == container.bestmaxtunnel && cumbitconditions[bit][k] + bc > container.bestpathcond) continue;
#endif
						bindex[bit] = k;
						break;
					}
			} else {
				for (unsigned k = 0; k < bitdataend[bit].size(); ++k)
					if (bitdataend[bit][k] == bitdatastart[bit+1][bindex[bit+1]])
					{ // we have a full path !!!
						bindex[0] = k;
#if 1
						if (cumtunnelstrength[bit][k] + ts < container.bestmaxtunnel) continue;
						if (cumtunnelstrength[bit][k] + ts == container.bestmaxtunnel && cumbitconditions[bit][k] + bc > container.bestpathcond) continue;
#endif
						for (unsigned b = 0; b < 32; ++b)
						{
							newpath2.setbitcondition(t-2,b,bitdatanewcond[b][bindex[b]][0]);
							newpath2.setbitcondition(t-1,b,bitdatanewcond[b][bindex[b]][1]);
							newpath2.setbitcondition(t+0,b,bitdatanewcond[b][bindex[b]][2]);
							newpath2.setbitcondition(t+1,b,bitdatanewcond[b][bindex[b]][3]);
							newpath2.setbitcondition(t+2,b,bitdatanewcond[b][bindex[b]][4]);
							newpath2.setbitcondition(t+3,b,bitdatanewcond[b][bindex[b]][5]);
						}
						container.push_back(newpath2);
					}
				bindex[bit] = bitdataend[bit].size();
			}
		} else
		{
			++bit;
			if (bit <= 31)
			{
#if 1
				unsigned ts = tsv[bit+1];
				unsigned bc = bcv[bit+1];
#endif
				++bindex[bit];
				if (bit < 31)
				{
					while (bindex[bit] < bitdataend[bit].size()
						&& (((bitdataend[bit][bindex[bit]]) != (bitdatastart[bit+1][bindex[bit+1]]))
#if 1
							|| (cumtunnelstrength[bit][bindex[bit]] + ts < container.bestmaxtunnel)
							|| (cumtunnelstrength[bit][bindex[bit]] + ts == container.bestmaxtunnel && cumbitconditions[bit][bindex[bit]] + bc > container.bestpathcond)
#endif
						))
						++bindex[bit];
				} else {
					while (bindex[bit] < bitdataend[bit].size()
#if 1
						&& ((cumtunnelstrength[bit][bindex[bit]] + ts < container.bestmaxtunnel)
							|| (cumtunnelstrength[bit][bindex[bit]] + ts == container.bestmaxtunnel && cumbitconditions[bit][bindex[bit]] + bc > container.bestpathcond)
						)
#endif
						)
						++bindex[bit];
				}
			}
		}
	}

	return 32;
}

void md5_connect_thread::md5_connect(const vector<differentialpath>& lowerpaths
				 , const differentialpath& upperpath
				 , path_container& container)
{
//	static timer sw(true);
//	static vector<unsigned> countb(33, 0);
//	static vector<unsigned> countbaborted(33, 0);
//	static vector<unsigned> countbdepth(33, 0);
//	static uint64 count, countall;
//	static vector<unsigned char> isgood;
//	static vector<int> lowerpathsmaxtunnel;
	if (lowerpathsmaxtunnel.size() == 0) {
		lowerpathsmaxtunnel.resize(lowerpaths.size());
		for (unsigned i = 0; i < lowerpaths.size(); ++i)
		{
			differentialpath tmp = lowerpaths[i];
		try {
		cleanup(tmp);
		} catch (std::exception& e) {
			cerr << "hashclash::cleanup(differentialpath&): unknown exception!:" << endl << e.what() << endl;
			show_path(tmp, container.m_diff);
		}catch (...) {
			cerr << "hashclash::cleanup(differentialpath&): unknown exception!:" << endl;
			show_path(tmp, container.m_diff);
		}

			//cleanup(tmp);
			lowerpathsmaxtunnel[i] = totaltunnelstrength(tmp);
		}
	}
        int uppercompl = 0;
        for (int k = upperpath.tbegin(); k < upperpath.tend() && k < 64; ++k)
        	if (k >= container.Qcondstart)
                	uppercompl -= int(upperpath[k].hw());
	if (container.showstats && sw.time() > 60)
	{
		cout << endl << count << "\t" << countall << "\t"
			<< double(countall)/double(count) << endl;
		for (unsigned b = 0; b <= 32; ++b)
		{
			cout << b << ":\t" << countb[b] << "\t" << countbaborted[b] <<
				"\t" << double(countbaborted[b])/double(countb[b]) <<
				"\t" << double(countbdepth[b])/double(countb[b]) << endl;
		}
		sw.start();
	}
	t = container.t;
	Ft = md5bf(t);
	Ftp1 = md5bf(t+1);
	Ftp2 = md5bf(t+2);
	Ftp3 = md5bf(t+3);
	dmt = container.m_diff[md5_wt[t]];
	dmtp1 = container.m_diff[md5_wt[t+1]];
	dmtp2 = container.m_diff[md5_wt[t+2]];
	dmtp3 = container.m_diff[md5_wt[t+3]];

	unsigned bequal = 0;
	if (0 != newpath.path.size())
	{
		while (bequal < 32
			&& newpath[t+1][bequal] == upperpath[t+1][bequal]
			&& newpath[t+2][bequal] == upperpath[t+2][bequal]
			&& newpath[t+3][bequal] == upperpath[t+3][bequal]
			)
			++bequal;
	}
	newpath = upperpath;

	isgood.resize(lowerpaths.size(),0);
	dFt.resize(lowerpaths.size());
	dFtp1.resize(lowerpaths.size());
	dFtp2.resize(lowerpaths.size());
	dFtp3.resize(lowerpaths.size());
	dQtp1 = upperpath[t+1].diff();
	dQtp2 = upperpath[t+2].diff();
	dQtp3 = upperpath[t+3].diff();
	dQtp4 = upperpath[t+4].diff();
	uint32 dTtp1 = best_rotated_difference(dQtp2 - dQtp1, 32-md5_rc[t+1]) - dmtp1;
	uint32 dTtp2 = best_rotated_difference(dQtp3 - dQtp2, 32-md5_rc[t+2]) - dmtp2;
	uint32 dTtp3 = best_rotated_difference(dQtp4 - dQtp3, 32-md5_rc[t+3]) - dmtp3;
	for (unsigned i = 0; i < lowerpaths.size(); ++i)
	{
		uint32 dQt = lowdQt[i];
		uint32 dTt = best_rotated_difference(dQtp1 - dQt, 32-md5_rc[t]);
		uint32 dFti = dTt - dmt - lowdQtm3[i];
		uint32 dFtp1i = dTtp1 - lowdQtm2[i];
		uint32 dFtp2i = dTtp2 - lowdQtm1[i];
		uint32 dFtp3i = dTtp3 - dQt;
		if (isgood[i] != 0)
		{
			if (isgood[i] <= bequal)
			{
				if (0 != ((dFti^dFt[i])<<(32-isgood[i]))
					|| 0 != ((dFtp1i^dFtp1[i])<<(32-isgood[i]))
					|| 0 != ((dFtp2i^dFtp2[i])<<(32-isgood[i]))
					|| 0 != ((dFtp3i^dFtp3[i])<<(32-isgood[i]))
					)
					isgood[i] = 0;
			} else
				isgood[i] = 0;
		}
		dFt[i] = dFti;
		dFtp1[i] = dFtp1i;
		dFtp2[i] = dFtp2i;
		dFtp3[i] = dFtp3i;
	}

	countall += lowerpaths.size();
	for (unsigned i = 0; i < lowerpaths.size(); ++i)
	{
		if (isgood[i]) continue;
		if (lowerpathsmaxtunnel[i]+uppercompl <= container.bestmaxcomp) continue;
		++count;
		unsigned b = md5_connect_bits(lowerpaths, i, upperpath, container);
		++countb[b];
#ifndef DONT_SKIP_LOWERPATHS
		// skip all lowerpaths that have the same characteristics on the b lower bits
		if (b < 32) {
			isgood[i] = b+1;

			unsigned j = i + 1;
			unsigned jmax = binary_search_lower_paths(lowerpaths, i, b) + 1;
			countbdepth[b] += jmax - j;

			uint32 maskt = 0;
			for (unsigned k = 0; k < b; ++k)
				maskt |= 1<<k;
			uint32 masktp1 = maskt;
			uint32 masktp2 = maskt;
			uint32 masktp3 = maskt;
			maskt |= 1<<b;
			if (lastdFp1) masktp1 |= 1<<b;
			if (lastdFp2) masktp2 |= 1<<b;
			if (lastdFp3) masktp3 |= 1<<b;

			uint32 dFti = dFt[i] & maskt;
			uint32 dFtp1i = dFtp1[i] & masktp1;
			uint32 dFtp2i = dFtp2[i] & masktp2;
			uint32 dFtp3i = dFtp3[i] & masktp3;
			for (; j < jmax; ++j)
			{
				// verify if dF for t,t+1,t+2,t+3 are equal on these bits
				// if so skip this path
				if ( dFti == (dFt[j]&maskt)
					&& dFtp1i == (dFtp1[j]&masktp1)
					&& dFtp2i == (dFtp2[j]&masktp2)
					&& dFtp3i == (dFtp3[j]&masktp3)
					)
				{
					isgood[j] = b+1;
					++countbaborted[b];
				}
			}
		}
#endif // DONT_SKIP_LOWERPATHS
	}
}


#include "ode/odeint.hpp"

/// Integrate routines
#include <boost/numeric/odeint/integrate/integrate.hpp>

namespace SOS {
    namespace ODE {
        namespace odeint = boost::numeric::odeint;

        Real Odeint::eval_ode(Ode_id ode_id_, Dt_id dt_id_,
                              Context&& context_) const
        {
            State x = move(context_.cx_init());
            auto f = [this, ode_id_, dt_id_]
                         (const State& x_, State& dx_, Time t_){
                         eval_ode_step(ode_id_, dt_id_,
                                       dx_[def_dt_id], x_, t_);
                     };
            integrate(f, x, context_.ct_init(), context_.ct_end(),
                      traject(ode_id_));
            return x[def_dt_id];
        }

        State Odeint::eval_unif_odes(Dt_ids&& dt_ids_,
                                     Context&& context_) const
        {
            State x = move(context_.cx_init());
            auto f = [this, &dt_ids_](const State& x_, State& dx_, Time t_){
                         eval_unif_odes_step(dt_ids_, dx_, x_, t_);
                     };
            integrate(f, x, context_.ct_init(), context_.ct_end(),
                      traject());
            return x;
        }

        size_t Odeint::integrate(Integrate_f f, State& x,
                                 Time t_init_, Time t_end_,
                                 Traject& traject_) const
        {
            auto observer = [&traject_](const State& x_, Time t_){
                traject_.add_step(x_, t_);
            };

            size_t n_steps = odeint::integrate(f, x,
                                               t_init_, t_end_,
                                               cstep_size(),
                                               observer);
            return n_steps;
        }
    }
}


////////////////////////////////////////////////////////////////////////////////////////////////////
// everywhere.cpp
//
//  Copyright 2013 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)

#include <boost/proto/v5/proto.hpp>
#include "./unit_test.hpp"

namespace proto = boost::proto;
using proto::_;

struct S_ {};
constexpr proto::expr<proto::terminal(S_)> S {};
constexpr auto omg = S(1,2)(2,3);

struct Square
  : proto::def<
        proto::everywhere(
            proto::case_(
                proto::terminal(int),
                proto::terminal(proto::multiplies(proto::_value, proto::_value))
            )
        )
    >
{};

using X = proto::custom<proto::expr<_>>;
constexpr X::function<
    X::function<
        X::terminal<S_>
      , X::terminal<int>
      , X::terminal<int>
    >
  , X::terminal<int>
  , X::terminal<int>
> omg_wtf = Square()(omg);

static_assert(proto::value(proto::child<2>(proto::child<0>(omg_wtf)))==4, "oops");

struct Flip
  : proto::def<
        proto::everywhere(
            proto::case_(
                proto::plus(_,_)
              , proto::minus(proto::_left, proto::_right)
            )
          , proto::case_(
                proto::minus(_,_)
              , proto::plus(proto::_left, proto::_right)
            )
        )
    >
{};

void test_everywhere()
{
    using namespace proto::literals;

    proto::expr<
        proto::plus(
            proto::minus(
                proto::plus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
              , proto::plus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
            )
          , proto::minus(
                proto::plus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
              , proto::plus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
            )
        )
    > y = ((1_et + 1_et) - (1_et + 1_et)) + ((1_et + 1_et) - (1_et + 1_et));

    proto::expr<
        proto::minus(
            proto::plus(
                proto::minus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
              , proto::minus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
            )
          , proto::plus(
                proto::minus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
              , proto::minus(
                    proto::terminal(unsigned long long)
                  , proto::terminal(unsigned long long)
                )
            )
        )
    > z = Flip()(y);

    BOOST_CHECK_EQUAL(
        proto::value(proto::left(proto::left(proto::left(z))))
      , 1
    );
}

using namespace boost::unit_test;
////////////////////////////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("testing the everywhere action");

    test->add(BOOST_TEST_CASE(&test_everywhere));

    return test;
}


//
// Created by wei on 1/21/19.
//

#include <Open3D/Open3D.h>
#include <Cuda/Geometry/NNCuda.h>
#include <Eigen/Eigen>
#include <gtest/gtest.h>

using namespace open3d;
using namespace open3d::utility;
using namespace open3d::cuda;

TEST(NN, NNSearch) {
    const int size = 10000;
    const int feature_size = 33;
    Eigen::MatrixXd qr = Eigen::MatrixXd::Zero(feature_size, size);
    Eigen::MatrixXd reference = Eigen::MatrixXd::Zero(feature_size, size);

    for (int i = 0; i < size; ++i) {
        qr(0, i) = i;
        qr(feature_size - 1, i) = i;
        reference(0, i) = i;
        reference(feature_size - 1, i) = i;
    }

    NNCuda nn;
    Timer timer;
    timer.Start();
    nn.BruteForceNN(qr, reference);
    timer.Stop();
    PrintInfo("NNSearch takes: %f\n", timer.GetDuration());

    auto result = nn.nn_idx_.Download();
    for (int i = 0; i < size; ++i) {
        EXPECT_EQ(i, result(0, i));
    }
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


#include <boost/test/unit_test.hpp>

#include <scorum/protocol/betting/market.hpp>

#include <defines.hpp>

namespace {
using namespace scorum;
using namespace scorum::protocol;

BOOST_AUTO_TEST_SUITE(threshold_comparison_tests)

BOOST_AUTO_TEST_CASE(total_goals_tests)
{
    /*
     * During over/under markets resolving you should check if value ('goals' in total market for example) is
     * greater then 'over' market and less then 'under' market
     */
    {
        int16_t threshold = 0;
        total::over over_05{ 500 };
        total::under under_05{ 500 };
        BOOST_CHECK(!(threshold > over_05.threshold));
        BOOST_CHECK((threshold < under_05.threshold));

        total::over over_10{ 1000 };
        total::under under_10{ 1000 };
        BOOST_CHECK(!(threshold > over_10.threshold));
        BOOST_CHECK((threshold < under_10.threshold));

        total::over over_15{ 1500 };
        total::under under_15{ 1500 };
        BOOST_CHECK(!(threshold > over_15.threshold));
        BOOST_CHECK((threshold < under_15.threshold));
    }
    {
        int goals = 1000;
        total::over over_05{ 500 };
        total::under under_05{ 500 };
        BOOST_CHECK((goals > over_05.threshold));
        BOOST_CHECK(!(goals < under_05.threshold));

        total::over over_10{ 1000 };
        total::under under_10{ 1000 };
        BOOST_CHECK(!(goals > over_10.threshold));
        BOOST_CHECK(!(goals < under_10.threshold));

        total::over over_15{ 1500 };
        total::under under_15{ 1500 };
        BOOST_CHECK(!(goals > over_15.threshold));
        BOOST_CHECK((goals < under_15.threshold));

        total::over over_20{ 2000 };
        total::under under_20{ 2000 };
        BOOST_CHECK(!(goals > over_20.threshold));
        BOOST_CHECK((goals < under_20.threshold));
    }
    {
        int goals = 2000;
        total::over over_05{ 500 };
        total::under under_05{ 500 };
        BOOST_CHECK((goals > over_05.threshold));
        BOOST_CHECK(!(goals < under_05.threshold));

        total::over over_10{ 1000 };
        total::under under_10{ 1000 };
        BOOST_CHECK((goals > over_10.threshold));
        BOOST_CHECK(!(goals < under_10.threshold));

        total::over over_15{ 1500 };
        total::under under_15{ 1500 };
        BOOST_CHECK((goals > over_15.threshold));
        BOOST_CHECK(!(goals < under_15.threshold));

        total::over over_20{ 2000 };
        total::under under_20{ 2000 };
        BOOST_CHECK(!(goals > over_20.threshold));
        BOOST_CHECK(!(goals < under_20.threshold));
    }
}

BOOST_AUTO_TEST_CASE(handicap_home_tests)
{
    {
        int home_minus_away_delta = 1000;
        handicap::over over_15{ 1500 };
        handicap::under under_15{ 1500 };
        BOOST_CHECK(!(home_minus_away_delta > over_15.threshold));
        BOOST_CHECK((home_minus_away_delta < under_15.threshold));

        handicap::over over_10{ 1000 };
        handicap::under under_10{ 1000 };
        BOOST_CHECK(!(home_minus_away_delta > over_10.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_10.threshold));

        handicap::over over_05{ 500 };
        handicap::under under_05{ 500 };
        BOOST_CHECK((home_minus_away_delta > over_05.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_05.threshold));

        handicap::over over_00{ 0 };
        handicap::under under_00{ 0 };
        BOOST_CHECK((home_minus_away_delta > over_00.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_00.threshold));
    }
    {
        int home_minus_away_delta = 0;

        handicap::over over_10{ 1000 };
        handicap::under under_10{ 1000 };
        BOOST_CHECK(!(home_minus_away_delta > over_10.threshold));
        BOOST_CHECK((home_minus_away_delta < under_10.threshold));

        handicap::over over_05{ 500 };
        handicap::under under_05{ 500 };
        BOOST_CHECK(!(home_minus_away_delta > over_05.threshold));
        BOOST_CHECK((home_minus_away_delta < under_05.threshold));

        handicap::over over_00{ 0 };
        handicap::under under_00{ 0 };
        BOOST_CHECK(!(home_minus_away_delta > over_00.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_00.threshold));

        handicap::over over_m05{ -500 };
        handicap::under under_m05{ -500 };
        BOOST_CHECK((home_minus_away_delta > over_m05.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_m05.threshold));

        handicap::over over_m10{ -1000 };
        handicap::under under_m10{ -1000 };
        BOOST_CHECK((home_minus_away_delta > over_m10.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_m10.threshold));
    }
    {
        int home_minus_away_delta = -1000;

        handicap::over over_05{ 500 };
        handicap::under under_05{ 500 };
        BOOST_CHECK(!(home_minus_away_delta > over_05.threshold));
        BOOST_CHECK((home_minus_away_delta < under_05.threshold));

        handicap::over over_00{ 0 };
        handicap::under under_00{ 0 };
        BOOST_CHECK(!(home_minus_away_delta > over_00.threshold));
        BOOST_CHECK((home_minus_away_delta < under_00.threshold));

        handicap::over over_m05{ -500 };
        handicap::under under_m05{ -500 };
        BOOST_CHECK(!(home_minus_away_delta > over_m05.threshold));
        BOOST_CHECK((home_minus_away_delta < under_m05.threshold));

        handicap::over over_m10{ -1000 };
        handicap::under under_m10{ -1000 };
        BOOST_CHECK(!(home_minus_away_delta > over_m10.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_m10.threshold));

        handicap::over over_m15{ -1500 };
        handicap::under under_m15{ -1500 };
        BOOST_CHECK((home_minus_away_delta > over_m15.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_m15.threshold));

        handicap::over over_m20{ -2000 };
        handicap::under under_m20{ -2000 };
        BOOST_CHECK((home_minus_away_delta > over_m20.threshold));
        BOOST_CHECK(!(home_minus_away_delta < under_m20.threshold));
    }
}

BOOST_AUTO_TEST_SUITE_END()
}


/* boost random/uniform_int.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_int.hpp 12208 2002-01-03 22:21:34Z jmaurer $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_INT_HPP
#define BOOST_RANDOM_UNIFORM_INT_HPP

#include <cassert>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/detail/signed_unsigned_compare.hpp>

namespace boost {

// uniform integer distribution on [min, max]
template<class UniformRandomNumberGenerator, class IntType = int>
class uniform_int
{
public:
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

  uniform_int(base_type & rng, IntType min, IntType max)
    : _rng(rng), _min(min), _max(max), _range(_max - _min),
      _bmin(_rng.min()), _brange(_rng.max() - _bmin)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
#endif
    assert(min < max);
    if(random::equal_signed_unsigned(_brange, _range))
      _range_comparison = 0;
    else if(random::lessthan_signed_unsigned(_brange, _range))
      _range_comparison = -1;
    else
      _range_comparison = 1;
  }
  result_type operator()();
  result_type min() const { return _min; }
  result_type max() const { return _max; }
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const uniform_int& x, const uniform_int& y)
  { return x._min == y._min && x._max == y._max && x._rng == y._rng; }
#else
  // Use a member function
  bool operator==(const uniform_int& rhs) const
  { return _min == rhs._min && _max == rhs._max && _rng == rhs._rng;  }
#endif
private:
  typedef typename base_type::result_type base_result;
  base_type & _rng;
  result_type _min, _max, _range;
  base_result _bmin, _brange;
  int _range_comparison;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UniformRandomNumberGenerator, class IntType>
const bool uniform_int<UniformRandomNumberGenerator, IntType>::has_fixed_range;
#endif

template<class UniformRandomNumberGenerator, class IntType>
inline IntType uniform_int<UniformRandomNumberGenerator, IntType>::operator()()
{
  if(_range_comparison == 0) {
    // this will probably never happen in real life
    // basically nothing to do; just take care we don't overflow / underflow
    return static_cast<result_type>(_rng() - _bmin) + _min;
  } else if(_range_comparison < 0) {
    // use rejection method to handle things like 0..3 --> 0..4
    for(;;) {
      // concatenate several invocations of the base RNG
      // take extra care to avoid overflows
      result_type limit;
      if(_range == std::numeric_limits<result_type>::max()) {
        limit = _range/(static_cast<result_type>(_brange)+1);
        if(_range % static_cast<result_type>(_brange)+1 == static_cast<result_type>(_brange))
          ++limit;
      } else {
        limit = (_range+1)/(static_cast<result_type>(_brange)+1);
      }
      // we consider "result" as expressed to base (_brange+1)
      // for every power of (_brange+1), we determine a random factor
      result_type result = 0;
      result_type mult = 1;
      while(mult <= limit) {
        result += (_rng() - _bmin) * mult;
        mult *= static_cast<result_type>(_brange)+1;
      }
      if(mult == limit)
        // _range+1 is an integer power of _brange+1: no rejections required
        return result;
      // _range/mult < _brange+1  -> no endless loop
      result += uniform_int<base_type,result_type>(_rng, 0, _range/mult)() * mult;
      if(result <= _range)
        return result + _min;
    }
  } else {                   // brange > range
    if(_brange / _range > 4 /* quantization_cutoff */ ) {
      // the new range is vastly smaller than the source range,
      // so quantization effects are not relevant
      return uniform_smallint<base_type,result_type>(_rng, _min, _max)();
    } else {
      // use rejection method to handle things like 0..5 -> 0..4
      for(;;) {
        base_result result = _rng() - _bmin;
        // result and range are non-negative, and result is possibly larger
        // than range, so the cast is safe
        if(result <= static_cast<base_result>(_range))
          return result + _min;
      }
    }
  }
}

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_INT_HPP


// Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>

// Version: 1.0
// Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl

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

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#include "rigidbodyinertia.hpp"

#include <Eigen/Core>

using namespace Eigen;

namespace KDL{

    const static bool mhi=true;

    RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi):
        m(m_),h(h_),I(I_)
    {
    }

    RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic):
        m(m_),h(m*c_){
        //I=Ic-c x c x
        Vector3d c_eig=Map<const Vector3d>(c_.data);
        Map<Matrix3d>(I.data)=Map<const Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
    }

    RigidBodyInertia operator*(double a,const RigidBodyInertia& I){
        return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi);
    }

    RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){
        return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
    }

    Wrench operator*(const RigidBodyInertia& I,const Twist& t){
        return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel);
    }

    RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I){
        Frame X=T.Inverse();
        //mb=ma
        //hb=R*(h-m*r)
        //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
        Vector hmr = (I.h-I.m*X.p);
        Vector3d r_eig = Map<Vector3d>(X.p.data);
        Vector3d h_eig = Map<const Vector3d>(I.h.data);
        Vector3d hmr_eig = Map<Vector3d>(hmr.data);
        Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
        Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
        Matrix3d R = Map<Matrix3d>(X.M.data);
        RotationalInertia Ib;
        Map<Matrix3d>(Ib.data) = R*((Map<const Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());

        return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
    }

    RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
        //mb=ma
        //hb=R*h
        //Ib = R(Ia)R' with r=0
        Matrix3d R = Map<const Matrix3d>(M.data);
        RotationalInertia Ib;
        Map<Matrix3d>(Ib.data) = R.transpose()*(Map<const Matrix3d>(I.I.data)*R);

        return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
    }

    RigidBodyInertia RigidBodyInertia::RefPoint(const Vector& p){
        //mb=ma
        //hb=(h-m*r)
        //Ib = (Ia+r x h x + (h-m*r) x r x)
        Vector hmr = (this->h-this->m*p);
        Vector3d r_eig = Map<const Vector3d>(p.data);
        Vector3d h_eig = Map<Vector3d>(this->h.data);
        Vector3d hmr_eig = Map<Vector3d>(hmr.data);
        Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
        Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
        RotationalInertia Ib;
        Map<Matrix3d>(Ib.data) = Map<Matrix3d>(this->I.data)+rcrosshcross+hmrcrossrcross;

        return RigidBodyInertia(this->m,hmr,Ib,mhi);
    }
}//namespace