//sudo apt-get install libtbb-dev
//sudo apt-get install libboost-dev

//#include <iostream>
#include <stdio.h>
#include <thread>
#include <chrono>
#include <list>
#include <atomic>
//#include <queue>          // std::priority_queue
//#include <vector>         // std::vector
#include <map>
#include <unordered_map>
#include <climits>
#include <stack>          // std::stack
#include <math.h>       /* pow */
#include <boost/random.hpp>
#include <sys/time.h>

#include "tbb/concurrent_queue.h"
#include <boost/lockfree/stack.hpp>
#include "tbb/concurrent_hash_map.h"

#include <tervel/containers/lf/stack/stack.h>
#include <tervel/containers/wf/hash-map/wf_hash_map.h>
#include "main.h"

#include "datastructures/unboundedsize_kfifo.h"
#include "datastructures/boundedsize_kfifo.h"

#define NUM_THRDS 32
//#define TEST_SIZE 100
unsigned int TEST_SIZE;

//Set one parameter to 1 and the rest 0
#define LINEARIZABILITY 0
#define SEQUENTIAL_CONSISTENCY 0
#define QUIESCENT_CONSISTENCY 0
#define QUASI_LINEARIZABILITY 1

#define K 2
#define num_segments 5

#define DEBUG_ 0

//Set one parameter to 1 and the rest 0
//#define TBB_QUEUE 0
//#define KFIFO_QUEUE 1
//#define BOOST_STACK 0
//#define TERVEL_STACK 0
//#define TBB_MAP 0
//#define TERVEL_MAP 0

unsigned int TBB_QUEUE;
unsigned int KFIFO_QUEUE;
unsigned int BOOST_STACK;
unsigned int TERVEL_STACK;
unsigned int TBB_MAP;
unsigned int TERVEL_MAP;

struct MyHashCompare {
    static size_t hash( int x ) {
        return x;
    }
    //! True if strings are equal
    static bool equal( int x, int y ) {
        return x==y;
    }
};

typedef enum Status
{
	PRESENT,
	ABSENT
}Status;

typedef enum Semantics
{
	FIFO,
	LIFO,
	SET,
	MAP,
	PRIORITY
}Semantics;

typedef enum Type
{
	PRODUCER,
	CONSUMER,
	READER,
	WRITER
}Type;

typedef struct Method
{
	int id;
	int process;
	//int item;
	int item_key;
	int item_val;
	Semantics semantics;
	Type type;
    long int invocation;
	long int response;
	int quiescent_period;
	bool status;

	Method(int _id, int _process, int _item_key, int _item_val, Semantics _semantics, Type _type, long int _invocation, long int _response, bool _status)
	{
		id = _id;
		process = _process;
		item_key = _item_key;
		item_val = _item_val;
		semantics = _semantics;
		type = _type;
		invocation = _invocation;
		response = _response;
		quiescent_period = -1;
		status = _status;
	}
}Method;

typedef struct Item
{
	int key;
	int value;
	//int sum;
	double sum;

	//READS needs a separate sum, check linearizability by taking ceiling of sum + sum_reads > 0

	long int numerator;
	long int denominator;

	double exponent;

	Status status;

	std::stack<int> promote_items;

	std::list<Method> demote_methods;

	std::map<long int,Method,bool(*)(long int,long int)>::iterator producer;

	//Failed Consumer
	double sum_f;
	long int numerator_f;
	long int denominator_f;
	double exponent_f;

	//Reader
	double sum_r;
	long int numerator_r;
	long int denominator_r;
	double exponent_r;

	Item(int _key)
	{
		key = _key;
		value = INT_MIN;
		sum = 0;
		numerator = 0;
		denominator = 1;
		exponent = 0;
		status = PRESENT;
		sum_f = 0;
		numerator_f = 0;
		denominator_f = 1;
		exponent_f = 0;
		sum_r = 0;
		numerator_r = 0;
		denominator_r = 1;
		exponent_r = 0;
	}

	Item(int _key, int _val)
	{
		key = _key;
		value = _val;
		sum = 0;
		numerator = 0;
		denominator = 1;
		exponent = 0;
		status = PRESENT;
		sum_f = 0;
		numerator_f = 0;
		denominator_f = 1;
		exponent_f = 0;
		sum_r = 0;
		numerator_r = 0;
		denominator_r = 1;
		exponent_r = 0;
	}

	void add_int(long int x)
	{
		//printf("Test add function\n");
		long int add_num = x * denominator;
		numerator = numerator + add_num;
		//printf("add_num = %ld, numerator/denominator = %ld\n", add_num, numerator/denominator);
		sum = (double) numerator/denominator;
		//sum = sum + x;
	}
	void sub_int(long int x)
	{
		//printf("Test add function\n");
		long int sub_num = x * denominator;
		numerator = numerator - sub_num;
		//printf("sub_num = %ld, numerator/denominator = %ld\n", sub_num, numerator/denominator);
		sum = (double) numerator/denominator;
		//sum = sum + x;
	}
	void add_frac(long int num, long int den)
	{
#if DEBUG_
		if(den == 0)
			printf("WARNING: add_frac: den = 0\n");
		if(denominator == 0)
			printf("WARNING: add_frac: 1. denominator = 0\n");
#endif
		if(denominator%den == 0)
		{
			numerator = numerator + num * denominator/den;
		} else if (den%denominator == 0) {
			numerator = numerator * den/denominator + num;
			denominator = den;
		} else {
			numerator = numerator * den + num * denominator;
			denominator = denominator * den;
		}
#if DEBUG_
		if(denominator == 0)
			printf("WARNING: add_frac: 2. denominator = 0\n");
#endif
		sum = (double) numerator/denominator;
	}

	void sub_frac(long int num, long int den)
	{
#if DEBUG_
		if(den == 0)
			printf("WARNING: sub_frac: den = 0\n");
		if(denominator == 0)
			printf("WARNING: sub_frac: 1. denominator = 0\n");
#endif
		if(denominator%den == 0)
		{
			numerator = numerator - num * denominator/den;
		} else if (den%denominator == 0) {
			numerator = numerator * den/denominator - num;
			denominator = den;
		} else {
			numerator = numerator * den - num * denominator;
			denominator = denominator * den;
		}
#if DEBUG_
		if(denominator == 0)
			printf("WARNING: sub_frac: 2. denominator = 0\n");
#endif
		sum = (double) numerator/denominator;
	}

	void demote()
	{
		exponent = exponent + 1;
		long int den = (long int) pow (2, exponent);
		//printf("denominator = %ld\n", den);
		sub_frac(1, den);
	}

	void promote()
	{
		long int den = (long int) pow (2, exponent);
#if DEBUG_
		if(den == 0)
			printf("2 ^ %f = %ld?\n", exponent, den);
#endif
		if(exponent < 0)
			den = 1;
		//printf("denominator = %ld\n", den);
		add_frac(1, den);
		exponent = exponent - 1;
	}

	//Failed Consumer
	void add_frac_f(long int num, long int den)
	{
#if DEBUG_
		if(den == 0)
			printf("WARNING: add_frac_f: den = 0\n");
		if(denominator_f == 0)
			printf("WARNING: add_frac_f: 1. denominator_f = 0\n");
#endif
		if(denominator_f%den == 0)
		{
			numerator_f = numerator_f + num * denominator_f/den;
		} else if (den%denominator_f == 0) {
			numerator_f = numerator_f * den/denominator_f + num;
			denominator_f = den;
		} else {
			numerator_f = numerator_f * den + num * denominator_f;
			denominator_f = denominator_f * den;
		}
#if DEBUG_
		if(denominator_f == 0)
			printf("WARNING: add_frac_f: 2. denominator_f = 0\n");
#endif
		sum_f = (double) numerator_f/denominator_f;
	}

	void sub_frac_f(long int num, long int den)
	{
#if DEBUG_
		if(den == 0)
			printf("WARNING: sub_frac_f: den = 0\n");
		if(denominator_f == 0)
			printf("WARNING: sub_frac_f: 1. denominator_f = 0\n");
#endif
		if(denominator_f%den == 0)
		{
			numerator_f = numerator_f - num * denominator_f/den;
		} else if (den%denominator_f == 0) {
			numerator_f = numerator_f * den/denominator_f - num;
			denominator_f = den;
		} else {
			numerator_f = numerator_f * den - num * denominator_f;
			denominator_f = denominator_f * den;
		}
#if DEBUG_
		if(denominator_f == 0)
			printf("WARNING: sub_frac_f: 2. denominator_f = 0\n");
#endif
		sum_f = (double) numerator_f/denominator_f;
	}

	void demote_f()
	{
		exponent_f = exponent_f + 1;
		long int den = (long int) pow (2, exponent_f);
		//printf("denominator = %ld\n", den);
		sub_frac_f(1, den);
	}

	void promote_f()
	{
		long int den = (long int) pow (2, exponent_f);
		//printf("denominator = %ld\n", den);
		add_frac_f(1, den);
		exponent_f = exponent_f - 1;
	}

	//Reader
	void add_frac_r(long int num, long int den)
	{
		if(denominator_r%den == 0)
		{
			numerator_r = numerator_r + num * denominator_r/den;
		} else if (den%denominator_r == 0) {
			numerator_r = numerator_r * den/denominator_r + num;
			denominator_r = den;
		} else {
			numerator_r = numerator_r * den + num * denominator_r;
			denominator_r = denominator_r * den;
		}
		sum_r = (double) numerator_r/denominator_r;
	}

	void sub_frac_r(long int num, long int den)
	{
		if(denominator_r%den == 0)
		{
			numerator_r = numerator_r - num * denominator_r/den;
		} else if (den%denominator_r == 0) {
			numerator_r = numerator_r * den/denominator_r - num;
			denominator_r = den;
		} else {
			numerator_r = numerator_r * den - num * denominator_r;
			denominator_r = denominator_r * den;
		}
		sum_r = (double) numerator_r/denominator_r;
	}

	void demote_r()
	{
		exponent_r = exponent_r + 1;
		long int den = (long int) pow (2, exponent_r);
		//printf("denominator = %ld\n", den);
		sub_frac_r(1, den);
	}

	void promote_r()
	{
		long int den = (long int) pow (2, exponent_r);
		//printf("denominator = %ld\n", den);
		add_frac_r(1, den);
		exponent_r = exponent_r - 1;
	}
}Item;

typedef struct Block
{
	long int start;
	long int finish;

	Block()
	{
		start = 0;
		finish = 0;
	}
}Block;

/*class Comparator
{
public:
  Comparator()
    {}
  bool operator() (Method lhs, Method rhs)
  {
  	return (lhs.response > rhs.response);
  }
};*/

FILE *pfile;

bool final_outcome;

unsigned int method_count;

bool fncomp (long int lhs, long int rhs) {return lhs < rhs;}

tbb::concurrent_queue<int> queue;
boost::lockfree::stack<int> stack(128);
tbb::concurrent_hash_map<int,int,MyHashCompare> map;

tervel::containers::lf::Stack<int> stack_tv;
tervel::containers::wf::HashMap<long int, long int> map_tv(32768, 5);
//tervel::containers::wf::HashMap<long int, long int>* map_tv;
tervel::Tervel* tervel_obj;
//scal::UnboundedSizeKFifo<long unsigned int> queue_kfifo((long unsigned int) K);
scal::UnboundedSizeKFifo<long unsigned int>* queue_kfifo;
//scal::BoundedSizeKFifo<long unsigned int>* queue_kfifo;
std::string s1 ("1g"); //TODO 1k or 1m or 1g
size_t tlsize;

std::list<Method> thrd_lists[NUM_THRDS];

std::atomic<int> thrd_lists_size[NUM_THRDS];

std::atomic<bool> done[NUM_THRDS];

std::atomic<int> barrier(0);

void wait()
{
	barrier.fetch_add(1);
	while(barrier.load() < NUM_THRDS) { }
}

long int method_time[NUM_THRDS];
long int overhead_time[NUM_THRDS];

std::chrono::time_point<std::chrono::high_resolution_clock> start;

long int elapsed_time_verify;

void set_quiescent_period(std::map<long int,Method,bool(*)(long int,long int)>& map_methods, std::map<long int,Block,bool(*)(long int,long int)>& map_block, std::map<long int,Method,bool(*)(long int,long int)>::iterator& it_begin)
{
	std::map<long int,Method,bool(*)(long int,long int)>::iterator it_m;
	std::map<long int,Block,bool(*)(long int,long int)>::iterator it_b;

	//for (it_m=map_methods.begin(); it_m!=map_methods.end(); ++it_m)
	//{
	for (it_m=it_begin; it_m!=map_methods.end(); ++it_m)
	{
		if(map_block.empty())
		{
			Block block;
			block.start = it_m->second.invocation;
			block.finish = it_m->second.response;

			map_block.insert ( std::pair<long int,Block>(block.finish,block) );
		}
		else {
			Block block;
			block.start = it_m->second.invocation;
			block.finish = it_m->second.response;

			for (it_b=map_block.begin(); it_b!=map_block.end(); ++it_b)
			{

				if(block.start <= it_b->second.start)
				{
					//it_b->second.start = it_m->second.invocation;
					//it_b->second.finish = it_m->second.response;

					map_block.erase(it_b);
				} else if (block.start <= it_b->second.finish)
				{
					//it_b->second.finish = it_m->second.response;
					block.start = it_b->second.start;
					map_block.erase(it_b);
				} else if (it_m->second.invocation > it_b->second.finish)
				{
					//Do nothing
					//map_block.insert ( std::pair<long int,Block>(block.response,block) );
				} else {
					//printf("Why am I here?\n");
				}
			}
			map_block.insert ( std::pair<long int,Block>(block.finish,block) );
		}
	}

	//for (it_m=map_methods.begin(); it_m!=map_methods.end(); ++it_m)
	//{
	for (it_m=it_begin; it_m!=map_methods.end(); ++it_m)
	{
		int period = 0;
		for (it_b=map_block.begin(); it_b!=map_block.end(); ++it_b)
		{
			if(it_m->second.invocation >= it_b->second.start && it_m->second.response <= it_b->second.finish)
			{
				it_m->second.quiescent_period = period;
				break;
			}
			period = period + 1;

		}
	}

}

void handle_failed_consumer(std::map<long int,Method,bool(*)(long int,long int)>& map_methods, std::unordered_map<int,Item>& map_items, std::map<long int,Method,bool(*)(long int,long int)>::iterator& it, std::unordered_map<int,Item>::iterator& it_item, std::stack<std::unordered_map<int,Item>::iterator>& stack_failed)
{
	std::map<long int,Method,bool(*)(long int,long int)>::iterator it_0;
	for (it_0=map_methods.begin(); it_0 != it; ++it_0)
	{
#if LINEARIZABILITY
		if(it_0->second.response < it->second.invocation)
#elif SEQUENTIAL_CONSISTENCY
		if((it_0->second.response < it->second.invocation) && (it_0->second.process == it->second.process))
#elif QUIESCENT_CONSISTENCY
		if(it_0->second.quiescent_period < it->second.quiescent_period)
#elif QUASI_LINEARIZABILITY
		int index = 0;
		std::map<long int,Method,bool(*)(long int,long int)>::iterator it_2;
		for (it_2=it_0; it_2 != it; ++it_2)
		{
			if(it_2->second.response < it->second.invocation)
			{
				index = index + 1;
				if(index > K)
					break;
			}
		}
		if(index > K)
#endif
		{
			std::unordered_map<int,Item>::iterator it_item_0;
			it_item_0 = map_items.find(it_0->second.item_key);
			if(it_0->second.type == PRODUCER && it_item->second.status == PRESENT && (it_0->second.semantics == FIFO || it_0->second.semantics == LIFO || it->second.item_key == it_0->second.item_key))
			{
				//it_item->second.promote_failed.push(it_item_0->second.key);
				//it_item_0->second.demote_f();
				//printf("handle_failed\n");
				stack_failed.push(it_item_0);
			}
		}
	}
}

void handle_failed_read(std::map<long int,Method,bool(*)(long int,long int)>& map_methods, std::unordered_map<int,Item>& map_items, std::map<long int,Method,bool(*)(long int,long int)>::iterator& it, std::unordered_map<int,Item>::iterator& it_item, std::stack<std::unordered_map<int,Item>::iterator>& stack_failed)
{
	std::map<long int,Method,bool(*)(long int,long int)>::iterator it_0;
	for (it_0=map_methods.begin(); it_0 != it; ++it_0)
	{
#if LINEARIZABILITY
		if(it_0->second.response < it->second.invocation)
#elif SEQUENTIAL_CONSISTENCY
		if((it_0->second.response < it->second.invocation) && (it_0->second.process == it->second.process))
#elif QUIESCENT_CONSISTENCY
		if(it_0->second.quiescent_period < it->second.quiescent_period)
#elif QUASI_LINEARIZABILITY
		int index = 0;
		std::map<long int,Method,bool(*)(long int,long int)>::iterator it_2;
		for (it_2=it_0; it_2 != it; ++it_2)
		{
			if(it_2->second.response < it->second.invocation)
			{
				index = index + 1;
				if(index > K)
					break;
			}
		}
		if(index > K)
#endif
		{
			std::unordered_map<int,Item>::iterator it_item_0;
			it_item_0 = map_items.find(it_0->second.item_key);
			if(it_0->second.type == PRODUCER && it_item->second.status == PRESENT && it->second.item_key == it_0->second.item_key)
			{
				//it_item->second.promote_failed.push(it_item_0->second.key);
				//it_item_0->second.demote_f();
				//printf("handle_failed_read\n");
				stack_failed.push(it_item_0);
			}
		}
	}
}

void verify_checkpoint(std::map<long int,Method,bool(*)(long int,long int)>& map_methods, std::unordered_map<int,Item>& map_items, std::map<long int,Method,bool(*)(long int,long int)>::iterator& it_start, long unsigned int& count_iterated, long int min, bool reset_it_start, std::map<long int,Block,bool(*)(long int,long int)>& map_block)
{
	if(!map_methods.empty())
	{
		//std::unordered_map<int,std::unordered_map<int,Item>::iterator> map_consumer;
		std::stack<std::unordered_map<int,Item>::iterator> stack_consumer;
		std::stack<std::map<long int,Method,bool(*)(long int,long int)>::iterator> stack_finished_methods;
		std::stack<std::unordered_map<int,Item>::iterator> stack_failed;

		//bool reset_it_start = true;
		std::map<long int,Method,bool(*)(long int,long int)>::iterator it;
		if(count_iterated == 0)
		{
			it=map_methods.begin();
			reset_it_start = false;
		//} else if(count_iterated < count_overall) {
		//} else if(count_iterated < map_methods.size()) {
		} else if(it != map_methods.end()) {
			it=++it_start;
		}

		for (; it!=map_methods.end(); ++it)
		{
			if(it->second.response > min)
				break;

			/*if(it->second.invocation > min)
			{
				min = it->second.response;
				it_start = it;
				break;
			}*/
			//fprintf(pfile, "Checking method %d\n", it->second.id);

			if(method_count%5000 == 0)
				printf("Method Count = %u\n", method_count);
			method_count = method_count + 1;

			it_start = it;
			reset_it_start = false;
			count_iterated = count_iterated + 1;

			std::unordered_map<int,Item>::iterator it_item;
			it_item = map_items.find(it->second.item_key);
			//(it_item->second).sum = (it_item->second).sum - 1;

#if DEBUG_
			if(it_item->second.status != PRESENT)
				printf("WARNING: Current item not present!\n");

			if((it->second).type == PRODUCER)
				printf("PRODUCER invocation %ld, response %ld, item %d\n", it->second.invocation, it->second.response, it->second.item_key);
			else if((it->second).type == CONSUMER)
				printf("CONSUMER invocation %ld, response %ld, item %d\n", it->second.invocation, it->second.response, it->second.item_key);
#endif

			if(it->second.type == PRODUCER)
			{
				it_item->second.producer = it;

				if(it_item->second.status == ABSENT)
				{
					//reset item parameters
					it_item->second.status = PRESENT;
					//printf("Clearing Item %d's demote_methods list\n", it_item->first);
					it_item->second.demote_methods.clear();
				}
				//(it_item->second).sum = (it_item->second).sum + 1;
				it_item->second.add_int(1);

				if(it->second.semantics == FIFO || it->second.semantics == LIFO)
				{
					std::map<long int,Method,bool(*)(long int,long int)>::iterator it_0;
					for (it_0=map_methods.begin(); it_0 != it; ++it_0)
					{
	#if LINEARIZABILITY
						if(it_0->second.response < it->second.invocation)
	#elif SEQUENTIAL_CONSISTENCY
						if((it_0->second.response < it->second.invocation) && (it_0->second.process == it->second.process))
	#elif QUIESCENT_CONSISTENCY
						if(it_0->second.quiescent_period < it->second.quiescent_period)
	#elif QUASI_LINEARIZABILITY
						int index = 0;
						std::map<long int,Method,bool(*)(long int,long int)>::iterator it_2;
						for (it_2=it_0; it_2 != it; ++it_2)
						{
							if(it_2->second.response < it->second.invocation)
							{
								index = index + 1;
								if(index > K)
									break;
							}
						}
						if(index > K)
	#endif
						{
							std::unordered_map<int,Item>::iterator it_item_0;
							it_item_0 = map_items.find(it_0->second.item_key);

							//Demotion
							//FIFO Semantics
							if((it_0->second.type == PRODUCER && it_item_0->second.status == PRESENT) && (it->second.type == PRODUCER) && (it_0->second.semantics == FIFO))
							{
								it_item_0->second.promote_items.push(it_item->second.key);
								it_item->second.demote();
								it_item->second.demote_methods.push_back(it_0->second);
							}

							//LIFO Semantics
							if((it_0->second.type == PRODUCER && it_item_0->second.status == PRESENT) && (it->second.type == PRODUCER) && (it_0->second.semantics == LIFO))
							{
								it_item->second.promote_items.push(it_item_0->second.key);
								it_item_0->second.demote();
								it_item_0->second.demote_methods.push_back(it->second);
								//printf("Pushing Item %d to Item %d's demote_method list\n", it->second.item_key, it_item_0->first);
							}

						}
					}
				}
			}
			if(it->second.type == READER) //&& (it->second.semantics == SET || it->second.semantics == MAP)
			{
				if(it->second.status == true)
				{
					it_item->second.demote_r();

				} else {
					//TODO: Probably should handle the same as a failed consumer
					handle_failed_read(map_methods, map_items, it, it_item, stack_failed);
				}

			}

			//if(((it->second).type == CONSUMER) && ((it->second).semantics == FIFO))
			if(it->second.type == CONSUMER)
			{
				/*std::unordered_map<int,std::unordered_map<int,Item>::iterator>::iterator it_consumer;
				it_consumer = map_consumer.find((it->second).key);
				if(it_consumer == map_consumer.end())
				{
					std::pair<int,std::unordered_map<int,Item>::iterator> entry ((it->second).key,it);
					//map_consumer.insert(std::make_pair<int,std::unordered_map<int,Item>::iterator>((it->second).key,it));
					map_consumer.insert(entry);
				} else {
					it_consumer->second = it_item_0;
				}*/
				if(it->second.status == true)
				{
					//PROMOTE READS
					if(it_item->second.sum > 0)
					{
						it_item->second.sum_r = 0;
					}

					it_item->second.sub_int(1);

					it_item->second.status = ABSENT;

					if(it_item->second.sum < 0)
					{
						//printf("WARNING: Sum < 0\n");
						std::list<Method>::iterator it_method;
						for(it_method = it_item->second.demote_methods.begin(); it_method != it_item->second.demote_methods.end(); ++it_method)
						{

							if((it->second.response < it_method->invocation) || (it_method->response < it->second.invocation))
							{
								//Methods do not overlap
								//printf("NOTE: Methods do not overlap\n");
							} else {
								//printf("NOTE: CONSUME Item %d overlaps with PRODUCE Item %d\n", it->second.item_val, it_method->item_val);
								it_item->second.promote();

								//Need to remove from promote list
								std::unordered_map<int,Item>::iterator it_method_item;
								it_method_item = map_items.find(it_method->item_key);
								std::stack<int> temp;
								while(!it_method_item->second.promote_items.empty())
								{
									int top = it_method_item->second.promote_items.top();
									if(top != it->second.item_key)
									{
										temp.push(top);
									}
									it_method_item->second.promote_items.pop();
									//printf("Stuck here?\n");
								}
								it_method_item->second.promote_items.swap(temp);

								it_method = it_item->second.demote_methods.erase(it_method);
								--it_method;

							}

						}
					}

					stack_consumer.push(it_item);

					//map_methods.erase(it); //TODO: Dangerous, make sure this is correct
					stack_finished_methods.push(it);
					//printf("stack_finished_methods.push(Consume(%d))\n", it->second.item_key);

					if(it_item->second.producer != map_methods.end())
					{
						stack_finished_methods.push(it_item->second.producer);
						//printf("stack_finished_methods.push(Produce(%d))\n", it_item->second.producer->second.item_key);
					}

				} else {
					handle_failed_consumer(map_methods, map_items, it, it_item, stack_failed);
				}
			}
		}
		if(reset_it_start)
		{
			--it_start;
		}

		//NEED TO FLAG ITEMS ASSOCIATED WITH CONSUMER METHODS AS ABSENT
		while(!stack_consumer.empty())
		{
			std::unordered_map<int,Item>::iterator it_top = stack_consumer.top();

			//(it_top->second).status = ABSENT;

			//std::unordered_map<int,Item>::iterator it_top = it_consumer->second;
			//printf("promote_items.size() = %lu\n", (it_top->second).promote_items.size());

			//printf("Item %d: Promote item ", it_top->first);
			//std::list<int>::iterator it_promote;
			int item_promote;
			//for(it_promote = (it_top->second).promote_items.begin(); it_promote != (it_top->second).promote_items.end(); ++it_promote)
			while(!it_top->second.promote_items.empty())
			{
				//TODO: Check that item is not ABSENT
				item_promote = it_top->second.promote_items.top();
				std::unordered_map<int,Item>::iterator it_promote_item;
				it_promote_item = map_items.find(item_promote);
				//printf("%d ", item_promote);
				//(it_promote_item->second).sum = (it_promote_item->second).sum + 1;
				//(it_promote_item->second).add_int(1);
				//(it_promote_item->second).add_frac(1, 2);
				it_promote_item->second.promote();
				it_top->second.promote_items.pop();
			}
			//printf("\n");

			stack_consumer.pop();
		}

		while(!stack_failed.empty())
		{
			std::unordered_map<int,Item>::iterator it_top = stack_failed.top();

			if(it_top->second.status == PRESENT)
			{
				it_top->second.demote_f();
			}

			stack_failed.pop();
		}

		//Remove methods that are no longer active

		while(!stack_finished_methods.empty())
		{
			std::map<long int,Method,bool(*)(long int,long int)>::iterator it_top = stack_finished_methods.top();
			map_methods.erase(it_top); //TODO: Dangerous, make sure this is correct
			stack_finished_methods.pop();
		}

		//Verify Sums
		std::unordered_map<int,Item>::iterator it_verify;

		bool outcome = true;
		for (it_verify = map_items.begin(); it_verify!=map_items.end(); ++it_verify)
		{
			if(it_verify->second.sum < 0)
			{
				outcome = false;
#if DEBUG_
				printf("WARNING: Item %d, sum %.2lf\n", it_verify->second.key, it_verify->second.sum);
#endif
			}
			//printf("Item %d, sum %.2lf\n", it_verify->second.key, it_verify->second.sum);

			if((ceil(it_verify->second.sum) + it_verify->second.sum_r) < 0)
			{
				outcome = false;
#if DEBUG_
				printf("WARNING: Item %d, sum_r %.2lf\n", it_verify->second.key, it_verify->second.sum_r);
#endif
			}

			int N;
			if(it_verify->second.sum_f == 0)
				N = 0;
			else
				N = -1;

			if(((ceil(it_verify->second.sum) + it_verify->second.sum_f) * N) < 0)
			{
				outcome = false;
#if DEBUG_
				printf("WARNING: Item %d, sum_f %.2lf\n", it_verify->second.key, it_verify->second.sum_f);
#endif
			}
		}
		if(outcome == true)
		{
			final_outcome = true;
#if DEBUG_
			printf("-------------Program Correct Up To This Point-------------\n");
#endif
		} else
		{
			final_outcome = false;
#if DEBUG_
			printf("-------------Program Not Correct-------------\n");
#endif
		}

	}
}

void work_queue(int id)
{

	//int keyRange = 20;
	unsigned int testSize = TEST_SIZE;

	double wallTime = 0.0;

	timeval tod;
	gettimeofday(&tod,0);
	wallTime += static_cast<double>(tod.tv_sec);
	wallTime += static_cast<double>(tod.tv_usec) * 1e-6;

    //boost::mt19937 randomGenKey;
    boost::mt19937 randomGenOp;
    //randomGenKey.seed(wallTime + id);
    randomGenOp.seed(wallTime + id + 1000);
    //boost::uniform_int<int> randomDistKey(1, keyRange);
    boost::uniform_int<unsigned int> randomDistOp(1, 100);

	auto start_time = std::chrono::time_point_cast<std::chrono::nanoseconds>(start);
	auto start_time_epoch = start_time.time_since_epoch();

	//int m_id = id;
	int m_id = id + 1;

	//printf("Spawning thread %d, start = %ld\n", id, start_time_epoch.count());

	std::chrono::time_point<std::chrono::high_resolution_clock> end;

if (KFIFO_QUEUE) {
	end = std::chrono::high_resolution_clock::now();

	auto pre_allocation = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
	auto pre_allocation_epoch = pre_allocation.time_since_epoch();
	long int invocation_alloc = pre_allocation_epoch.count() - start_time_epoch.count();

	//size_t tlsize = scal::HumanSizeToPages(s1.c_str(), s1.size());
	scal::ThreadLocalAllocator::Get().Init(tlsize, true);

	end = std::chrono::high_resolution_clock::now();

	auto post_allocation = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
	auto post_allocation_epoch = post_allocation.time_since_epoch();
	long int response_alloc = post_allocation_epoch.count() - start_time_epoch.count();

	overhead_time[id] = response_alloc - invocation_alloc;
}

	wait();
	for(unsigned int i = 0; i < testSize; i++)
	{
		Type type;
		int item_key = -1;
		bool res = true;

		uint32_t op_dist = randomDistOp(randomGenOp);

		end = std::chrono::high_resolution_clock::now();

		auto pre_function = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
		auto pre_function_epoch = pre_function.time_since_epoch();

		//auto invocation =
		//std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
		//printf("Thread %d invocation: %ld nanoseconds\n", id, invocation.count());

		long int invocation = pre_function_epoch.count() - start_time_epoch.count();

		//if(invocation.count() > (LONG_MAX - 10000000000)) //10 seconds until time exceeds limit
		if(invocation > (LONG_MAX - 10000000000)) //10 seconds until time exceeds limit
		{
#if DEBUG_
			printf("WARNING: TIME LIMIT REACHED! TERMINATING PROGRAM\n");
#endif
			break;
		}

		if(op_dist <= 50)
		//if(op_dist <= 33)
		{
			type = CONSUMER;
			int item_pop;
			long unsigned int* item_pop_ptr;
if (TBB_QUEUE) {
			res = queue.try_pop (item_pop);
} else if (KFIFO_QUEUE) {
			item_pop_ptr = (long unsigned int*) malloc (sizeof(long unsigned int));
			res = queue_kfifo->dequeue(item_pop_ptr);
}
			if(res)
			{
if (TBB_QUEUE) {
				item_key = item_pop;
} else if (KFIFO_QUEUE) {
				item_key = (int) *item_pop_ptr;
}
			} else
				item_key = INT_MIN;
		} else {
			type = PRODUCER;
			//item_key = randomDistKey(randomGenKey);
			item_key = m_id;
if (TBB_QUEUE) {
			queue.push(item_key);
} else if (KFIFO_QUEUE) {
			res = queue_kfifo->enqueue((long unsigned int) item_key);
}
		}

		end = std::chrono::high_resolution_clock::now();

		auto post_function = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
		auto post_function_epoch = post_function.time_since_epoch();

		//printf("Thread %d, item %d: prefunction = %ld, postfunction = %ld\n", id, item_key, pre_function_epoch.count(), post_function_epoch.count());

		//auto response =
		//std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);

		long int response = post_function_epoch.count() - start_time_epoch.count();

		//printf("Thread %d response: %ld nanoseconds\n", id, response.count());

		/*Method m1;
		m1.id = m_id;
		m1.process = id;

		m1.item_key = item_key;
		m1.type = type;
		m1.semantics = FIFO;
		m1.invocation = invocation.count();
		m1.response = response.count();*/

		//Method m1(m_id, id, item_key, FIFO, type, invocation.count(), response.count(), res);
		Method m1(m_id, id, item_key, INT_MIN, FIFO, type, invocation, response, res);

		m_id = m_id + NUM_THRDS;

		thrd_lists[id].push_back(m1);

		thrd_lists_size[id].fetch_add(1);

		//method_time[id] = method_time[id] + (response.count() - invocation.count());
		method_time[id] = method_time[id] + (response - invocation);

	}

	done[id].store(true);
}

void work_stack(int id)
{
	if(TERVEL_STACK || TERVEL_MAP)
	{
		tervel::ThreadContext* thread_context __attribute__((unused));
		thread_context = new tervel::ThreadContext(tervel_obj);
	}

	//int keyRange = 20;
	unsigned int testSize = TEST_SIZE;

	double wallTime = 0.0;

	timeval tod;
	gettimeofday(&tod,0);
	wallTime += static_cast<double>(tod.tv_sec);
	wallTime += static_cast<double>(tod.tv_usec) * 1e-6;

    //boost::mt19937 randomGenKey;
    boost::mt19937 randomGenOp;
    //randomGenKey.seed(wallTime + id);
    randomGenOp.seed(wallTime + id + 1000);
    //boost::uniform_int<int> randomDistKey(1, keyRange);
    boost::uniform_int<unsigned int> randomDistOp(1, 100);

	auto start_time = std::chrono::time_point_cast<std::chrono::nanoseconds>(start);
	auto start_time_epoch = start_time.time_since_epoch();

	int m_id = id;

	std::chrono::time_point<std::chrono::high_resolution_clock> end;

	wait();
	for(unsigned int i = 0; i < testSize; i++)
	{
		Type type;

		int item_key = -1;

		bool res = true;

		uint32_t op_dist = randomDistOp(randomGenOp);

		end = std::chrono::high_resolution_clock::now();

		auto pre_function = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
		auto pre_function_epoch = pre_function.time_since_epoch();

		//auto invocation =
		//std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
		//printf("Thread %d invocation: %ld nanoseconds\n", id, invocation.count());

		long int invocation = pre_function_epoch.count() - start_time_epoch.count();

		//if(invocation.count() > (LONG_MAX - 10000000000)) //10 seconds until time exceeds limit
		if(invocation > (LONG_MAX - 10000000000)) //10 seconds until time exceeds limit
		{
#if DEBUG_
			printf("WARNING: TIME LIMIT REACHED! TERMINATING PROGRAM\n");
#endif
			break;
		}

		if(op_dist <= 50)
		{
			type = CONSUMER;
			int item_pop;
if (BOOST_STACK) {
			res = stack.pop (item_pop);
} else if (TERVEL_STACK) {
			res = stack_tv.pop(item_pop);
}
			if(res)
				item_key = item_pop;
			else
				item_key = INT_MIN;

		} else {
			type = PRODUCER;
			item_key = m_id;
if (BOOST_STACK) {
			stack.push(item_key);
} else if (TERVEL_STACK) {
			stack_tv.push(item_key);
}
		}

		end = std::chrono::high_resolution_clock::now();

		auto post_function = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
		auto post_function_epoch = post_function.time_since_epoch();

		//printf("Thread %d, item %d: prefunction = %ld, postfunction = %ld\n", id, item_key, pre_function_epoch.count(), post_function_epoch.count());

		//auto response =
		//std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
		//printf("Thread %d response: %ld nanoseconds\n", id, response.count());

		long int response = post_function_epoch.count() - start_time_epoch.count();

		/*Method m1;
		m1.id = m_id;

		m1.item_key = item_key;
		m1.process = id;

		m1.type = type;
		//m1.semantics = FIFO;

		m1.semantics = LIFO;
		m1.invocation = invocation.count();

		m1.response = response.count();*/

		//Method m1(m_id, id, item_key, LIFO, type, invocation.count(), response.count(), res);
		Method m1(m_id, id, item_key, INT_MIN, LIFO, type, invocation, response, res);

		m_id = m_id + NUM_THRDS;

		thrd_lists[id].push_back(m1);

		thrd_lists_size[id].fetch_add(1);

		//method_time[id] = method_time[id] + (response.count() - invocation.count());
		method_time[id] = method_time[id] + (response - invocation);

	}

	done[id].store(true);
}

void work_map(int id)
{
	if(TERVEL_STACK || TERVEL_MAP)
	{
		tervel::ThreadContext* thread_context __attribute__((unused));
		thread_context = new tervel::ThreadContext(tervel_obj);
	}

	//int keyRange = 20;
	unsigned int testSize = TEST_SIZE;

	double wallTime = 0.0;

	timeval tod;
	gettimeofday(&tod,0);
	wallTime += static_cast<double>(tod.tv_sec);
	wallTime += static_cast<double>(tod.tv_usec) * 1e-6;

    //boost::mt19937 randomGenKey;
    boost::mt19937 randomGenOp;
    //randomGenKey.seed(wallTime + id);
    randomGenOp.seed(wallTime + id + 1000);
    //boost::uniform_int<int> randomDistKey(1, keyRange);
    boost::uniform_int<unsigned int> randomDistOp(1, 100);

	auto start_time = std::chrono::time_point_cast<std::chrono::nanoseconds>(start);
	auto start_time_epoch = start_time.time_since_epoch();

	int m_id = id;

	std::chrono::time_point<std::chrono::high_resolution_clock> end;

	wait();
	for(unsigned int i = 0; i < testSize; i++)
	{
		Type type;

		int item_key = -1;

		int item_val = -1;

		bool res = true;

		uint32_t op_dist = randomDistOp(randomGenOp);

		end = std::chrono::high_resolution_clock::now();

		auto pre_function = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
		auto pre_function_epoch = pre_function.time_since_epoch();

		//auto invocation =
		//std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
		//printf("Thread %d invocation: %ld nanoseconds\n", id, invocation.count());

		long int invocation = pre_function_epoch.count() - start_time_epoch.count();

		//if(invocation.count() > (LONG_MAX - 10000000000)) //10 seconds until time exceeds limit
		if(invocation > (LONG_MAX - 10000000000)) //10 seconds until time exceeds limit
		{
#if DEBUG_
			printf("WARNING: TIME LIMIT REACHED! TERMINATING PROGRAM\n");
#endif
			break;
		}

//if (TBB_MAP) {
		tbb::concurrent_hash_map<int,int,MyHashCompare>::accessor a;
//} else if (TERVEL_MAP) {
		tervel::containers::wf::HashMap<long int, long int>::ValueAccessor va;
//}

		if(op_dist <= 33)
		{
			type = CONSUMER;
			int item_erase = m_id - 2*NUM_THRDS;
if (TBB_MAP) {
			res = map.erase( item_erase );
} else if (TERVEL_MAP) {
			res = map_tv.remove((long int) item_erase);
}
			if(res)
			{
				item_key = item_erase;
			} else {
				item_key = INT_MIN;
			}

		} else if (op_dist <= 66) {
			type = PRODUCER;
			item_key = m_id;
			item_val = m_id;
if (TBB_MAP) {
			map.insert(a, item_key);
			a->second = item_val;
} else if (TERVEL_MAP) {
			map_tv.insert((long int) item_key, (long int) item_val);
}

		} else {
			type = READER;
			item_key = m_id - NUM_THRDS;
if (TBB_MAP) {
			res = map.find(a, item_key);
} else if (TERVEL_MAP) {
			res = map_tv.at((long int) item_key, va);
}
			if(res)
			{
if (TBB_MAP) {
				item_val = a->second;
} else if (TERVEL_MAP) {
				item_val = *(va.value());
}
			} else {
				item_key = INT_MIN;
				item_val = INT_MIN;
			}
		}

		end = std::chrono::high_resolution_clock::now();

		auto post_function = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
		auto post_function_epoch = post_function.time_since_epoch();

		//printf("Thread %d, item %d: prefunction = %ld, postfunction = %ld\n", id, item_key, pre_function_epoch.count(), post_function_epoch.count());

		//auto response =
		//std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
		//printf("Thread %d response: %ld nanoseconds\n", id, response.count());

		long int response = post_function_epoch.count() - start_time_epoch.count();

		/*Method m1;
		m1.id = m_id;
		m1.item_key = item_key;
		m1.process = id;
		m1.type = type;
		//m1.semantics = FIFO;
		m1.semantics = LIFO;
		m1.invocation = invocation.count();
		m1.response = response.count();*/

		//Method m1(m_id, id, item_key, LIFO, type, invocation.count(), response.count(), res);
		Method m1(m_id, id, item_key, item_val, MAP, type, invocation, response, res);

		m_id = m_id + NUM_THRDS;

		thrd_lists[id].push_back(m1);

		thrd_lists_size[id].fetch_add(1);

		//method_time[id] = method_time[id] + (response.count() - invocation.count());
		method_time[id] = method_time[id] + (response - invocation);

	}

	done[id].store(true);
}

void test_map(int id)
{
	tervel::ThreadContext* thread_context __attribute__((unused));
	thread_context = new tervel::ThreadContext(tervel_obj);

	long int m_id = (long int) id;

	for(int i = 0; i < 1000; i++)
	{
		long int item_key;

		long int item_val;

		item_key = m_id*1000;
		item_val = m_id;
		map_tv.insert(item_key, item_val);

		m_id = m_id + NUM_THRDS;

	}
}

void verify()
{
	wait();
	auto start_time = std::chrono::time_point_cast<std::chrono::nanoseconds>(start);
	auto start_time_epoch = start_time.time_since_epoch();

	std::chrono::time_point<std::chrono::high_resolution_clock> end;

	end = std::chrono::high_resolution_clock::now();

	auto pre_verify = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
	auto pre_verify_epoch = pre_verify.time_since_epoch();

	long int verify_start = pre_verify_epoch.count() - start_time_epoch.count();

	//std::priority_queue<Method,std::vector<Method>,Comparator> pq_methods;
	bool(*fn_pt)(long int,long int) = fncomp;
  	std::map<long int,Method,bool(*)(long int,long int)> map_methods (fn_pt);
	std::map<long int,Block,bool(*)(long int,long int)> map_block (fn_pt);

	std::unordered_map<int,Item> map_items;

	std::map<long int,Method,bool(*)(long int,long int)>::iterator it_start;

	std::list<Method>::iterator it[NUM_THRDS];

	int it_count[NUM_THRDS];

	bool stop = false;

	long int min;

	long unsigned int count_overall = 0;

	long unsigned int count_iterated = 0;

	//min = LONG_MAX;

	long int old_min;

#if QUIESCENT_CONSISTENCY
	long int earliest_response;
#endif
	std::map<long int,Method,bool(*)(long int,long int)>::iterator it_qstart;

	while(!stop)
	{
		stop = true;
		min = LONG_MAX;
#if QUIESCENT_CONSISTENCY
		earliest_response = LONG_MAX;
#endif
		for(int i = 0; i < NUM_THRDS; i++)
		{
			if(done[i].load() == false)
			{
				stop = false;
			}

			long int response_time = 0;
			while(it_count[i] < thrd_lists_size[i].load())
			{
				if(it_count[i] == 0)
				{
					it[i] = thrd_lists[i].begin();
				} else {
					++it[i];
				}

				Method m = *it[i];

				//if(m.item_key%500 == 0)
					//printf("Checking method %d\n", m.item_key);

				//pq_methods.push(m);

				//Consider doing a left shift by bits to store number of threads, and store thread id in lower bits
				std::map<long int,Method,bool(*)(long int,long int)>::iterator it_method;
				it_method = map_methods.find(m.response);
  				while (it_method != map_methods.end())
				{
					m.response = m.response + 1;
					it_method = map_methods.find(m.response);
				}
				response_time = m.response;

				map_methods.insert ( std::pair<long int,Method>(m.response,m) );

				//printf("Insert method %d\n", m.id);

#if QUIESCENT_CONSISTENCY
				if(m.response < earliest_response)
				{
					earliest_response = m.response;
					it_qstart = map_methods.find(m.response);
				}
#endif

				it_count[i] = it_count[i] + 1;

				count_overall = count_overall + 1;

				std::unordered_map<int,Item>::iterator it_item;
				it_item = map_items.find(m.item_key);
				if(it_item == map_items.end())
				{
					/*Item item;
					item.key = m.item_key;
					item.sum = 0;
					item.numerator = 0;
					item.denominator = 1;
					item.exponent = 0;
					item.status = PRESENT;*/
					Item item(m.item_key);
					item.producer = map_methods.end();
					map_items.insert(std::pair<int,Item>(m.item_key,item) );
					it_item = map_items.find(m.item_key);
				}

				/*if(m.type == PRODUCER)
				{
					//TODO: If Item is ABSENT, set it to PRESENT
					if(it_item->second.status == ABSENT)
					{
						it_item->second.status = PRESENT;
					}
					//(it_item->second).sum = (it_item->second).sum + 1;
					it_item->second.add_int(1);

				} else if(m.type == CONSUMER)
				{
					//(it_item->second).sum = (it_item->second).sum - 1;
					it_item->second.sub_int(1);
				}*/
			}

			if(response_time < min)
			{
				min = response_time;
			}
		}
		//printf("min = %ld\n", min);

#if QUIESCENT_CONSISTENCY
		if(earliest_response != LONG_MAX)
			set_quiescent_period(map_methods, map_block, it_qstart);
#endif

		verify_checkpoint(map_methods, map_items, it_start, count_iterated, min, true, map_block);
		/*if(min == old_min)
		{
			min = LONG_MAX;
		}*/

	}

	verify_checkpoint(map_methods, map_items, it_start, count_iterated, LONG_MAX, false, map_block);

#if DEBUG_
	printf("Count overall = %lu, count iterated = %lu, map_methods.size() = %lu\n", count_overall, count_iterated, map_methods.size());
#endif

#if DEBUG_
	printf("All threads finished!\n");

	/*std::map<long int,Method,bool(*)(long int,long int)>::iterator it_m;
	for (it_m=map_methods.begin(); it_m!=map_methods.end(); ++it_m)
	{
		set_quiescent_period(map_methods, map_block, it_m);
	}*/

	std::map<long int,Block,bool(*)(long int,long int)>::iterator it_b;
	for (it_b=map_block.begin(); it_b!=map_block.end(); ++it_b)
	{
		printf("Block start = %ld, finish = %ld\n", it_b->second.start, it_b->second.finish);
	}

	std::map<long int,Method,bool(*)(long int,long int)>::iterator it_;
	for (it_=map_methods.begin(); it_!=map_methods.end(); ++it_)
	{
		std::unordered_map<int,Item>::iterator it_item;
		it_item = map_items.find(it_->second.item_key);
		if(it_->second.type == PRODUCER)
			printf("PRODUCER inv %ld, res %ld, item %d, sum %.2lf, sum_r = %.2lf, sum_f = %.2lf, tid = %d, qperiod = %d\n", it_->second.invocation, it_->second.response, it_->second.item_key, it_item->second.sum, it_item->second.sum_r, it_item->second.sum_f, it_->second.process, it_->second.quiescent_period);
		else if ((it_->second).type == CONSUMER)
			printf("CONSUMER inv %ld, res %ld, item %d, sum %.2lf, sum_r = %.2lf, sum_f = %.2lf, tid = %d, qperiod = %d\n", it_->second.invocation, it_->second.response, it_->second.item_key, it_item->second.sum, it_item->second.sum_r, it_item->second.sum_f, it_->second.process, it_->second.quiescent_period);
		else if ((it_->second).type == READER)
			printf("READER inv %ld, res %ld, item %d, sum %.2lf, sum_r = %.2lf, sum_f = %.2lf, tid = %d, qperiod = %d\n", it_->second.invocation, it_->second.response, it_->second.item_key, it_item->second.sum, it_item->second.sum_r, it_item->second.sum_f, it_->second.process, it_->second.quiescent_period);
	}
#endif

	end = std::chrono::high_resolution_clock::now();

	auto post_verify = std::chrono::time_point_cast<std::chrono::nanoseconds>(end);
	auto post_verify_epoch = post_verify.time_since_epoch();
	long int verify_finish = post_verify_epoch.count() - start_time_epoch.count();

	elapsed_time_verify = verify_finish - verify_start;
}

int main(int argc,char* argv[])
{
	pfile = fopen("output.txt", "a");

	method_count = 0;

	TBB_QUEUE = 0;
	KFIFO_QUEUE = 0;
	BOOST_STACK = 0;
	TERVEL_STACK = 0;
	TBB_MAP = 0;
	TERVEL_MAP = 0;
	if( argc == 2 ) {
		printf("Test size = %d\n", atoi(argv[1]));
		TEST_SIZE = (unsigned int) atoi(argv[1]);
		TBB_QUEUE = 1;
		printf("Testing TBB_QUEUE\n");
	} else if (argc == 3) {
		printf("Test size = %d\n", atoi(argv[1]));
		TEST_SIZE = (unsigned int) atoi(argv[1]);
		if(atoi(argv[2]) == 0)
		{
			TBB_QUEUE = 1;
			printf("Testing TBB_QUEUE\n");
		} else if(atoi(argv[2]) == 1)
		{
			KFIFO_QUEUE = 1;
			printf("Testing KFIFO_QUEUE\n");
		} else if(atoi(argv[2]) == 2)
		{
			BOOST_STACK = 1;
			printf("Testing BOOST_STACK\n");
		} else if(atoi(argv[2]) == 3)
		{
			TERVEL_STACK = 1;
			printf("Testing TERVEL_STACK\n");
		} else if(atoi(argv[2]) == 4)
		{
			TBB_MAP = 1;
			printf("Testing TBB_MAP\n");
		} else if(atoi(argv[2]) == 5)
		{
			TERVEL_MAP = 1;
			printf("Testing TERVEL_MAP\n");
		}

	} else { //default
		printf("Test size = 10\n");
		TEST_SIZE = 10;
		TBB_QUEUE = 1;
		printf("Testing TBB_QUEUE\n");
	}

	if(TERVEL_STACK || TERVEL_MAP)
	{
		tervel_obj = new tervel::Tervel(NUM_THRDS+1);
		tervel::ThreadContext* thread_context __attribute__((unused));
		thread_context = new tervel::ThreadContext(tervel_obj);
		//map_tv = new tervel::containers::wf::HashMap<long int, long int>(32768, 5);
	}

if (KFIFO_QUEUE) {
	tlsize = scal::HumanSizeToPages(s1.c_str(), s1.size());
	scal::ThreadLocalAllocator::Get().Init(tlsize, true);
	//scal::ThreadContext::prepare(NUM_THRDS + 1);
  	//scal::ThreadContext::assign_context();

	queue_kfifo = new scal::UnboundedSizeKFifo<long unsigned int>((long unsigned int) K);
	//queue_kfifo = new scal::BoundedSizeKFifo<long unsigned int>((long unsigned int) K, (long unsigned int) num_segments);
}

	final_outcome = true;

	std::thread t[NUM_THRDS];

	std::thread v;

	start = std::chrono::high_resolution_clock::now();

	for(int i = 0; i < NUM_THRDS; i++)
	{
		if(TBB_QUEUE || KFIFO_QUEUE)
		{
			t[i] = std::thread(work_queue,i);
		} else if(BOOST_STACK || TERVEL_STACK)
		{
			t[i] = std::thread(work_stack,i);
		} else if(TBB_MAP || TERVEL_MAP)
		{
			t[i] = std::thread(work_map,i);
		}
		//t[i] = std::thread(test_map,i);
	}

	v = std::thread(verify);

	for(int i = 0; i < NUM_THRDS; i++)
	{
		t[i].join();
	}
	//TODO: Uncomment line 1426
	v.join();

	if(final_outcome == true)
	{
		printf("-------------Program Correct Up To This Point-------------\n");
	} else {
		printf("-------------Program Not Correct-------------\n");
	}

	auto finish = std::chrono::high_resolution_clock::now();
	auto elapsed_time = std::chrono::duration_cast<std::chrono::nanoseconds>(finish - start);
	double elapsed_time_double = elapsed_time.count()*0.000000001;
	printf("Total Time: %.15lf seconds\n", elapsed_time_double);
	//auto elapsed_time = std::chrono::duration_cast<std::chrono::seconds>(finish - start);
	//std::cout << elapsed_time.count() << std::endl;

	long int elapsed_time_method = 0;
	long int elapsed_overhead_time = 0;

	for(int i = 0; i < NUM_THRDS; i++)
	{
		if(method_time[i] > elapsed_time_method)
			elapsed_time_method = method_time[i];
		if(overhead_time[i] > elapsed_overhead_time)
			elapsed_overhead_time = overhead_time[i];
		//elapsed_time_method = elapsed_time_method + method_time[i];
		//elapsed_overhead_time = elapsed_overhead_time + overhead_time[i];
	}

	double elapsed_time_method_double = elapsed_time_method*0.000000001;
	double elapsed_overhead_time_double = elapsed_overhead_time*0.000000001;
	double elapsed_time_verify_double = elapsed_time_verify*0.000000001;

	printf("Total Method Time: %.15lf seconds\n", elapsed_time_method_double);
	printf("Total Overhead Time: %.15lf seconds\n", elapsed_overhead_time_double);

	//double elapsed_time_verify_double = elapsed_time_double - elapsed_time_method_double;
	//double elapsed_time_verify_double = elapsed_time_double - elapsed_time_method_double - elapsed_overhead_time_double;
	elapsed_time_verify_double = elapsed_time_verify_double - elapsed_time_method_double;

	printf("Total Verification Time: %.15lf seconds\n", elapsed_time_verify_double);

	fprintf(pfile, "%.15lf %.15lf\n", elapsed_time_method_double, elapsed_time_verify_double);
	fclose(pfile);

if (TBB_QUEUE) {
	printf("Final Queue Configuration:\n");
	typedef tbb::concurrent_queue<int>::iterator iter;
    for(iter i(queue.unsafe_begin()); i!=queue.unsafe_end(); i++)
        printf("%d ", *i);
    printf("\n");
} else if (KFIFO_QUEUE) {
	printf("Final kFIFO Queue Configuration:\n");
	long unsigned int* item_pop_ptr = (long unsigned int*) malloc (sizeof(long unsigned int));
	while(queue_kfifo->dequeue(item_pop_ptr))
	{
		printf("%d ", (int) *item_pop_ptr);
	}
	printf("\n");
} else if (BOOST_STACK) {
	printf("Final Stack Configuration:\n");
	int stack_val;
	while(stack.pop(stack_val))
	{
		printf("%d ", stack_val);
	}
	printf("\n");
} else if (TERVEL_STACK) {
	printf("Final Tervel Stack Configuration:\n");
	int stack_val_tv;
	while(stack_tv.pop(stack_val_tv))
	{
		printf("%d ", stack_val_tv);
	}
	printf("\n");
} else if (TBB_MAP) {
	printf("Final Map Configuration:\n");
	tbb::concurrent_hash_map<int,int,MyHashCompare>::iterator it;
	for( it=map.begin(); it!=map.end(); ++it )
    	printf("%d,%d ",it->first,it->second);
	printf("\n");
} else if (TERVEL_MAP) {
	printf("Final Tervel Map Configuration:\n");
	tervel::containers::wf::HashMap<long int, long int>::ValueAccessor va;
	for(int i = 0; i < 100; i++)
	{
		bool res = map_tv.at((long int) i, va);
		if(res)
		{
			printf("%d,%ld ",i,*(va.value()));
		}
	}
	printf("\n");
}
    return 0;
}


#include "VelocityFilter.h"
#include <iostream>
#include <fstream>
#include <vector>
#include "Distance.h"
#include <Eigen/Core>
#include "CTCD.h"

using namespace std;
using namespace Eigen;

static void writeMesh(const char *filename, const VectorXd &verts, const Matrix3Xi &faces)
{
	ofstream ofs(filename);

	for(int i=0; i<verts.size()/3; i++)
	{
		ofs << "v ";
		for(int j=0; j<3; j++)
			ofs << verts[3*i+j] << " ";
		ofs << endl;
	}

	for(int i=0; i<faces.cols(); i++)
	{
		ofs << "f ";
		for(int j=0; j<3; j++)
			ofs << faces.coeff(j, i)+1 << " ";
		ofs << endl;
	}
}

void loadMesh(const char *filename, VectorXd &verts, Matrix3Xi &faces)
{
	ifstream ifs(filename);
	if(!ifs)
		return;

	vector<double> pos;
	vector<int> faceidx;

	while(true)
	{
		char c;
		ifs >> c;
		if(!ifs)
			break;
		if(c == 'v')
		{
			for(int i=0; i<3; i++)
			{
				double p;
				ifs >> p;
				pos.push_back(p);
			}
		}
		if(c == 'f')
		{
			for(int i=0; i<3; i++)
			{
				int vert;
				ifs >> vert;
				faceidx.push_back(vert);
			}
		}
	}

	int nverts = pos.size()/3;
	int nfaces = faceidx.size()/3;

	verts.resize(3*nverts);
	for(int i=0; i<3*nverts; i++)
		verts[i] = pos[i];

	faces.resize(3, nfaces);
	for(int i=0; i<nfaces; i++)
		for(int j=0; j<3; j++)
			faces.coeffRef(j, i) = faceidx[3*i+j]-1;
}

double displaceInward(const VectorXd &q, const Matrix3Xi &f, const Matrix3Xd &vertNormals, double maxDisplacement, VectorXd &displacedq)
{
	int nverts = q.size()/3;
	int nfaces = f.cols();
	displacedq = q;
	double displacement = maxDisplacement;

	for(int i=0; i<nverts; i++)
	{
		Vector3d pos = q.segment<3>(3*i);
		Vector3d normal = vertNormals.col(i);
		Vector3d endpos = pos - maxDisplacement*normal;

		// vertex-face

		for(int j=0; j<nfaces; j++)
		{
			bool skip = false;
			for(int k=0; k<3; k++)
				if(f.col(j)[k] == i)
					skip = true;

			if(skip)
				continue;

			double t;
			if(CTCD::vertexFaceCTCD(pos,
					q.segment<3>(3*f.col(j)[0]),
					q.segment<3>(3*f.col(j)[1]),
					q.segment<3>(3*f.col(j)[2]),
					endpos,
					q.segment<3>(3*f.col(j)[0]),
					q.segment<3>(3*f.col(j)[1]),
					q.segment<3>(3*f.col(j)[2]),
					1e-8, t))
			{
				displacement = min(displacement, maxDisplacement*t/2);
			}
		}
		// edge-edge

		for(int j=0; j<nfaces; j++)
		{
			for(int k=0; k<3; k++)
			{
				if(f.col(j)[k] == i)
				{
					int vert2 = f.col(j)[(k+1)%3];
					Vector3d pos2 = q.segment<3>(3*vert2);
					Vector3d endpos2 = pos2 - maxDisplacement*vertNormals.col(vert2);
					for(int j2=0; j2<nfaces; j2++)
					{
						for(int k2=0; k2<3; k2++)
						{
							if(f.col(j2)[k2] == i || f.col(j2)[(k2+1)%3] == i)
								continue;
							if(f.col(j2)[k2] == vert2 || f.col(j2)[(k2+1)%3] == vert2)
								continue;

							double t;
							if(CTCD::edgeEdgeCTCD(pos, pos2,
								q.segment<3>(3*f.col(j2)[k2]),
								q.segment<3>(3*f.col(j2)[(k2+1)%3]),
								endpos, endpos2,
								q.segment<3>(3*f.col(j2)[k2]),
								q.segment<3>(3*f.col(j2)[(k2+1)%3]),
								1e-8, t))
							{
								displacement = min(displacement, maxDisplacement*t/2);
							}
						}
					}
				}
			}
		}
	}

	for(int i=0; i<nverts; i++)
	{
		displacedq.segment<3>(3*i) -= displacement*vertNormals.col(i);
	}

	return displacement;
}

int main(int argc, char *argv[])
{
	if(argc != 4)
	{
		std::cerr << "Usage: displaceInward (inputMesh) (outputMesh) (maxDisplacement)" << std::endl;
		return -1;
	}
	VectorXd q;
	Matrix3Xi f;
	loadMesh(argv[1], q, f);
	int numverts = q.size()/3;
	std::cout << "Loaded mesh with " << numverts << " vertices and " << f.cols() << " faces" << std::endl;

	double maxdisp = strtod(argv[3], NULL);

	Matrix3Xd normals(3, numverts);
	normals.setZero();
	for(int face=0; face<f.cols(); face++)
	{
		Vector3i verts = f.col(face);
		Vector3d n = ( q.segment<3>(3*verts[1]) - q.segment<3>(3*verts[0]) ).cross( q.segment<3>(3*verts[2]) - q.segment<3>(3*verts[0]) );
		n /= n.norm();

		for(int i=0; i<3; i++)
		{
			double sinangle = ( q.segment<3>(3*verts[(i+1)%3]) - q.segment<3>(3*verts[i]) ).cross( q.segment<3>(3*verts[(i+2)%3]) - q.segment<3>(3*verts[i]) ).norm();
			double cosangle = ( q.segment<3>(3*verts[(i+1)%3]) - q.segment<3>(3*verts[i]) ).dot( q.segment<3>(3*verts[(i+2)%3]) - q.segment<3>(3*verts[i]) );
			double angle = atan2(sinangle, cosangle);
			normals.col(verts[i]) += angle*n;
		}
	}
	for(int i=0; i<numverts; i++)
		normals.col(i) /= normals.col(i).norm();

	VectorXd newq;
	double mindisp = displaceInward(q, f, normals, maxdisp, newq);
	std::cout << "Actual displacement was " << mindisp << std::endl;
	writeMesh(argv[2], newq, f);
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <iostream>
#include <boost/property_map.hpp>

int
main()
{
  using namespace boost;
  double x[] = { 0.2, 4.5, 3.2 };
  iterator_property_map < double *, identity_property_map, double, double& > pmap(x);
  std::cout << "x[1] = " << get(pmap, 1) << std::endl;
  put(pmap, 0, 1.7);
  std::cout << "x[0] = " << pmap[0] << std::endl;
  return 0;
}


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

#include <boost/property_tree/ptree.hpp>
#include <boost/random/bernoulli_distribution.hpp>
#include <cstring>
#include <iostream>

#include "clotho/recombination/recombination_rate_parameter.hpp"
#include "clotho/recombination/sequence_bias_parameter.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#ifdef DEBUGGING
#define ASSERT_VALID_RANGE( x, min, max ) assert( min <= x && x < max );
#else
#define ASSERT_VALID_RANGE( x, min, max )
#endif

namespace clotho {
namespace genetics {

template < class RNG, class PositionType >
class crossover_event_generator {
public:
    typedef RNG             random_engine_type;
    typedef PositionType    position_type;
    typedef PositionType    event_type;
    typedef unsigned int    bin_type;

    typedef typename position_distribution_helper< PositionType >::type position_distribution_type;

    typedef crossover_event_distribution_helper< double > event_distribution_helper_type;

    typedef boost::random::bernoulli_distribution< double > sequence_bias_distribution_type;

    typedef typename event_distribution_helper_type::IntType    IntType;
    typedef typename event_distribution_helper_type::type       event_distribution_type;

    typedef position_type *                         position_vector;
    typedef event_type *                            event_vector;
    typedef bin_type *                              bin_vector;

    static const unsigned int BIN_MAX = 256;

    crossover_event_generator( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rand( rng )
        , m_pos( NULL )
        , m_bins( NULL )
        , m_events( NULL )
        , m_events_size(0)
        , m_event_alloc(0)
        , m_base_seq(0)
        , m_bin_size(0)
        , m_alloc_size(0)
    {
        recombination_rate_parameter< double > rho( config);

        m_event_dist.param( typename event_distribution_type::param_type( rho.m_rho ) );

        sequence_bias_parameter< double > bias( config );
        m_seq_bias.param( typename sequence_bias_distribution_type::param_type( bias.m_bias ) );
    }

    void update( position_vector pos, size_t N ) {
        m_pos = pos;

        resizeBins( N );

        size_t M = N % 4;
        size_t i = 0;
        while( i < M ) {
            m_bins[ i ] = m_pos[i] * BIN_MAX;
            i += 1;
        }

        while( i < N ) {
            m_bins[ i ] = m_pos[i] * BIN_MAX;
            m_bins[ i + 1 ] = m_pos[ i + 1 ] * BIN_MAX;
            m_bins[ i + 2 ] = m_pos[ i + 2 ] * BIN_MAX;
            m_bins[ i + 3 ] = m_pos[ i + 3 ] * BIN_MAX;
            i += 4;
        }
    }

    // generate a new event distribution
    size_t generate() {
        // clear the counts
        IntType N = m_event_dist( *m_rand );    // generate the maximum number of events

        setBaseSequence( m_seq_bias( *m_rand ) );

        resize( N );

        size_t prev = 0;
        position_type accum = 0.0;

        size_t i = 0;
        position_type e = (position_type)(N);
        while( e > (position_type) 0 ) {
            position_type p = m_pos_dist( *m_rand );
            accum += log( p ) / e;

            p = (1.0 - exp( accum ));

            assert( i == 0 || m_events[ i - 1 ] < p);

            size_t cur = p * BIN_MAX;   // transform the current event to its specific bin
            while( prev < cur ) {
                m_counts[ prev++ ] = i;
            }

            m_events[ i++ ] = p;
            m_counts[ cur ] = i;
            prev = cur;
            e -= 1.0;
        }

        while( prev <= BIN_MAX ) {
            m_counts[ prev++ ] = i;
        }

        assert( i == N );
        return N;
    }

    // test whether the genetic position at the given index
    //
    bool operator()( size_t index ) {
        ASSERT_VALID_RANGE( index, 0, m_bin_size )

        position_type p = m_pos[ index ];
        bin_type bin_index = m_bins[index];

        ASSERT_VALID_RANGE( bin_index, 0, BIN_MAX );

        size_t hi = m_counts[ bin_index ];
        size_t lo =  ((bin_index == 0) ? 0 : m_counts[ bin_index - 1]);

        while( lo < hi && m_events[lo] < p ){ ++lo; }

        return (m_base_seq ^ (lo & 1));    // % 2
    }

    void setBaseSequence( bool is_base ) {
        m_base_seq = ((is_base) ? 0 : 1);
    }

    size_t getBaseSequence() {
        return m_base_seq;
    }

    virtual ~crossover_event_generator() {
        if( m_events != NULL ) {
            delete [] m_events;
        }

        if( m_bins != NULL ) {
            delete [] m_bins;
        }
    }

protected:

    void resize( size_t e ) {
        if( e > m_event_alloc ) {
            if( m_events != NULL ) {
                delete [] m_events;
            }
            m_events = new event_type[ e ];

            m_event_alloc = e;
        }

        m_events_size = e;
    }

    void resizeBins( size_t N ) {
        if( N > m_alloc_size ) {
            if( m_bins != NULL ) {
                delete [] m_bins;
            }

            m_bins = new bin_type[ N ];
            m_alloc_size = N;
        }

        m_bin_size = N;
    }

    random_engine_type          * m_rand;
    event_distribution_type     m_event_dist;
    position_distribution_type  m_pos_dist;

    position_vector     m_pos;
    bin_vector          m_bins;

    event_vector        m_events;
    size_t              m_events_size, m_event_alloc;

    size_t              m_counts[ BIN_MAX + 1 ];

    sequence_bias_distribution_type m_seq_bias;
    size_t              m_base_seq;
    size_t              m_bin_size, m_alloc_size;
};

#undef ASSERT_VALID_RANGE

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_CROSSOVER_EVENT_GENERATOR_HPP_


//          Copyright Gavin Band 2008 - 2012.
// 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 <string>
#include <iomanip>
#include <boost/math/distributions/binomial.hpp>
#include <boost/function.hpp>
#include <Eigen/Core>
#include "genfile/Error.hpp"
#include "components/HaplotypeFrequencyComponent/HaplotypeFrequencyComponent.hpp"

// #define DEBUG_HAPLOTYPE_FREQUENCY_LL 1
HaplotypeFrequencyLogLikelihood::HaplotypeFrequencyLogLikelihood(
	Matrix const& genotype_table,
	Matrix const& haplotype_table
):
	m_genotype_table( genotype_table ),
	m_haplotype_table( haplotype_table ),
	m_D_ll( 3 ),
	m_DDt_ll( 3, 3 )
{
	if( m_genotype_table.array().maxCoeff() == 0 && m_haplotype_table.array().maxCoeff() == 0 ) {
		throw genfile::BadArgumentError( "HaplotypeFrequencyLogLikelihood::HaplotypeFrequencyLogLikelihood()", "genotype_table, haplotype_table" ) ;
	}
	// store derivatives of elements of the parameters pi with respect to pi.
	m_dpi.push_back( std::vector< RowVector >( 2, RowVector::Zero( 3 ) )) ;
	m_dpi.push_back( std::vector< RowVector >( 2, RowVector::Zero( 3 ) )) ;
	m_dpi[0][0] = RowVector::Constant( 3, -1 ) ;
	m_dpi[0][1]( 0 ) = 1 ;
	m_dpi[1][0]( 1 ) = 1 ;
	m_dpi[1][1]( 2 ) = 1 ;
}

HaplotypeFrequencyLogLikelihood::Vector HaplotypeFrequencyLogLikelihood::get_MLE_by_EM() const {
	//
	// This method treats the number of individuals X in the G(1,1) cell
	// that have AB/ab genotypes as binomially distributed with some unknown
	// parameter p that must be estimated.  We pick a value of p and sum over
	// possible values of X to get a new estimate of the four parameters
	// pi00, pi01, pi10, pi11.  Then p is re-computed from the parameter values.
	//
	// We stop when the parameter estimate does not change, up to some tolerance.
	//
	Matrix const& G = m_genotype_table ;
	Vector pi = estimate_parameters( 0.5 ) ;
#if DEBUG_HAPLOTYPE_FREQUENCY_LL
	std::cerr << std::resetiosflags( std::ios::floatfield ) << "Maximising likelihood for genotypes:\n"
		<< m_genotype_table
		<< "\nand haplotypes:\n"
		<< m_haplotype_table
		<< "\n" ;
	std::cerr << "pi = " << pi(0) << " " << pi(1) << " " << pi(2) << " " << pi(3) << " " << ", p = 0.5.\n" ;
#endif
	if( G(1,1) != 0.0 ) {
		Vector old_pi ;
		std::size_t count = 0 ;
		std::size_t const max_count = 100000 ;
		double const tolerance = 0.0000001 ;
		do {
			old_pi = pi ;

			double p = ( pi(0) * pi( 3 ) ) ;
			if( p != 0 ) {
				p = p / ( p + ( pi( 1 ) * pi( 2 )) ) ;
			}

			pi = estimate_parameters( p ) ;
#if DEBUG_HAPLOTYPE_FREQUENCY_LL
			std::cerr << "pi = " << pi(0) << " " << pi(1) << " " << pi(2) << " " << pi(3) << " " << ", p = " << p << ".\n" ;
#endif
		}
		while( ( pi - old_pi ).array().abs().maxCoeff() > tolerance && ++count < max_count ) ;
		if( count == max_count ) {
			throw genfile::OperationFailedError(
				"HaplotypeFrequencyLogLikelihood::maximise_by_EM()",
				"object of type HaplotypeFrequencyLogLikelihood",
				"convergence"
			) ;
		}
	}
	return pi.tail( 3 ) ;
}

HaplotypeFrequencyLogLikelihood::Vector HaplotypeFrequencyLogLikelihood::estimate_parameters(
	double const p
) const {
	Matrix const& G = m_genotype_table ;
	Matrix const& H = m_haplotype_table ;

	// p is proportion of het / het genotypes
	// that are due to 00+11 haplotype combinations.
	double expected_00_11 = G( 1, 1 ) * p ;
	double expected_01_10 = G( 1, 1 ) - expected_00_11;

	// params are pi00 pi01 pi10 pi11
	Vector result( 4 ) ;
	result <<
		H(0,0) + G( 0, 1 ) + 2 * G( 0, 0 ) + G( 1, 0 ) + expected_00_11,
		H(0,1) + G( 0, 1 ) + 2 * G( 0, 2 ) + G( 1, 2 ) + expected_01_10,
		H(1,0) + G( 2, 1 ) + 2 * G( 2, 0 ) + G( 1, 0 ) + expected_01_10,
		H(1,1) + G( 1, 2 ) + 2 * G( 2, 2 ) + G( 2, 1 ) + expected_00_11
	;
	result /= result.sum() ;
	return result ;
}

void HaplotypeFrequencyLogLikelihood::evaluate_at( Vector const& pi ) {
	double const pi00 = 1.0 - pi.sum() ;
	double const& pi01 = pi(0) ;
	double const& pi10 = pi(1) ;
	double const& pi11 = pi(2) ;

	using std::log ;
	double const lpi00 = log( pi00 ) ;
	double const lpi01 = log( pi01 ) ;
	double const lpi10 = log( pi10 ) ;
	double const lpi11 = log( pi11 ) ;

	double het_probability = pi00 * pi11 + pi01 * pi10 ;

	Matrix const& G = m_genotype_table ;
	Matrix const& H = m_haplotype_table ;
	Matrix VG( 3, 3 ) ;
	VG <<
		2.0 * lpi00,		lpi00 + lpi01,				2.0 * lpi01,
		lpi00 + lpi10,		log( het_probability ),		lpi01 + lpi11,
		2.0 * lpi10,		lpi10 + lpi11,				2.0 * lpi11
	;

	Matrix VH( 2, 2 ) ;
	VG <<
		lpi00,		lpi01,
		lpi10,		lpi11
	;

	// cells with count 0 contribute 0 to the loglikelihood/
	// Since log(0) = -inf we have to handle this specially.
	// We do this by replacing these parameter entries with 0.
	VG = VG.array() * ( G.array() > 0.0 ).cast< double >() ;
	VH = VH.array() * ( H.array() > 0.0 ).cast< double >() ;

	m_ll = (
		(G.array() * VG.array())
		+ (H.array() * VH.array())
	).sum() ;
}
double HaplotypeFrequencyLogLikelihood::get_value_of_function() const {
	return m_ll ;
}

HaplotypeFrequencyLogLikelihood::Vector HaplotypeFrequencyLogLikelihood::get_value_of_first_derivative() {
	assert(0) ;
	return m_D_ll ;
}

HaplotypeFrequencyLogLikelihood::Matrix HaplotypeFrequencyLogLikelihood::get_value_of_second_derivative() {
	assert(0) ;
	return m_DDt_ll ;
}


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#pragma once

#include <Eigen/Dense>

#include <geo/ConvexPolytope.hpp>
#include <math/AffineSubspace.hpp>
#include <math/NatSet.hpp>

#include <forward_list>
#include <iostream>
#include <queue>
#include <stdexcept>
#include <unordered_map>
#include <unordered_set>
#include <vector>

namespace sfcpp {
namespace geo {

template <typename... Args>
std::function<void(Args...)> getDummyFunction() {
  return std::function<void(Args...)>([](Args...) {});
}

enum class FaceStatus { REMOVE, EXTEND, CONNECT, KEEP };

struct FaceInfo;

typedef FaceInfo *FaceInfoPtr;

static const double QH_EPSILON =
    1e-9;  // TODO: make relative instead of absolute

struct FaceInfo {
  math::NatSet vertexSet;

  std::unordered_set<FaceInfoPtr> parents;
  std::unordered_set<FaceInfoPtr> children;
  std::unordered_set<FaceInfoPtr> facesToConnect;

  // number of facets this face is belonging to
  uint32_t numAncestorFacets = 0;

  // number of ancestor facets where the currently processed point is on the
  // other side of the facet's hyperplane compared to the current polytope
  uint32_t p = 0;

  // number of ancestor facets where the currently processed point is inside the
  // facet's hyperplane
  uint32_t z = 0;

  bool shouldRemove = false;

  // for generation of Polytope
  size_t polytopeIndex = 0;  // corresponding index in polytope.faces[d] vector

  bool visited = false;

  virtual ~FaceInfo() {}

  FaceStatus getStatus() const {
    uint32_t n = numAncestorFacets - p - z;
    if (n > 0) {
      if (p > 0) {
        return FaceStatus::CONNECT;
      } else {
        return FaceStatus::KEEP;
      }
    } else {
      if (p > 0) {
        return FaceStatus::REMOVE;
      } else {
        return FaceStatus::EXTEND;
      }
    }
  }

  static std::function<void(FaceInfoPtr)> dummyFunction;

  bool isVertex() const { return vertexSet.size() == 1; }
};

struct FacetInfo : public FaceInfo {
  Eigen::VectorXd outerNormalVector;

  // dot product of any point of the face with outerNormalVector
  double offset = 0.0;

  std::vector<uint32_t> outsideSet;

  virtual ~FacetInfo() {}

  double signedDistance(Eigen::VectorXd v) const {
    return v.dot(outerNormalVector) - offset;
  }

  int distanceSign(Eigen::VectorXd v) {
    double signedDist = signedDistance(v);
    return signedDist > QH_EPSILON ? 1 : (signedDist < -QH_EPSILON ? -1 : 0);
  }
};

template <typename Set, typename T>
void removeFromSet(Set &set, T t) {
  auto it = set.find(t);
  if (it != set.end()) {
    set.erase(it);
  }
}

typedef FacetInfo *FacetInfoPtr;

/**
 * A custom implementation of the QuickHull algorithm that computes all faces of
 * the given polytope and also works if the facets are non-simplicial (which is
 * important for cube-based SFC, for example). This algorithm works in arbitrary
 * dimensions >= 2.
 */
class QuickHullAlgorithm {
  std::vector<Eigen::VectorXd> vertices;

  // an inner point of the polytope used compute the direction of facet normal
  // vectors
  Eigen::VectorXd innerPoint;
  size_t dim;

  std::unordered_set<FaceInfoPtr> vertexFaces;
  std::unordered_set<FacetInfoPtr> facets;
  std::vector<FacetInfoPtr> aboveFacets, insideFacets, newFacets;

  std::queue<FacetInfoPtr> unprocessedFacets;
  std::unordered_set<FaceInfoPtr> connectFaces;
  std::unordered_set<FaceInfoPtr> newConnectFaces;

  std::shared_ptr<ConvexPolytope> polytope;

  void visitChildrenWithFlag(
      FaceInfoPtr face, bool visitedValue,
      std::function<void(FaceInfoPtr)> const &func = FaceInfo::dummyFunction);
  void visitChildren(FaceInfoPtr face,
                     std::function<void(FaceInfoPtr)> const &func);

  void visitParentsWithFlag(
      FaceInfoPtr face, bool visitedValue,
      std::function<void(FaceInfoPtr)> const &func = FaceInfo::dummyFunction);

  template <typename Collection>
  void visitChildrenForEach(Collection const &collection,
                            std::function<void(FaceInfoPtr)> const &func) {
    for (auto elem : collection) {
      visitChildrenWithFlag(elem, true, func);
      visitChildrenWithFlag(elem, false);
    }
  }

  template <typename Collection>
  void visitChildrenOnce(Collection const &collection,
                         std::function<void(FaceInfoPtr)> const &func) {
    for (auto elem : collection) {
      visitChildrenWithFlag(elem, true, func);
    }
    for (auto elem : collection) {
      visitChildrenWithFlag(elem, false);
    }
  }

  void initializeSimplex();

  void computeNeighbors(uint32_t pointIndex, FacetInfoPtr facet);

  void prepareConnect(FaceInfoPtr face,
                      std::unordered_set<FaceInfoPtr> const &otherFaces);

  void deleteFaceRecursively(FaceInfoPtr face);
  void deleteChildrenRecursively(FaceInfoPtr face);

  void addPoint(uint32_t pointIndex, FacetInfoPtr facet);

  void compute();

  void addToPolytopeRecursively(FaceInfoPtr face, size_t d = 0);

  template <typename T>
  void setFacetNormal(FacetInfoPtr facet, T const &vertexIndices) {
    if (vertexIndices.size() == 0) {
      throw std::runtime_error(
          "QuickHullAlgorithm::setFacetNormal(): vertexIndices.size() == 0");
    }
    auto it = vertexIndices.begin();
    auto firstPoint = this->vertices[*it];
    math::AffineSubspace affSpace(firstPoint);

    ++it;
    for (; it != vertexIndices.end(); ++it) {
      affSpace.addVector(this->vertices[*it]);
    }

    auto vec = affSpace.orthogonalVector().normalized();

    if (vec.dot(firstPoint - innerPoint) < 0) {
      vec *= -1;
    }

    facet->outerNormalVector = vec;
    facet->offset = vec.dot(firstPoint);
  }

 public:
  QuickHullAlgorithm(Eigen::MatrixXd points);

  ~QuickHullAlgorithm();

  std::shared_ptr<ConvexPolytope> createPolytope();
};

} /* namespace geo */
} /* namespace sfcpp */


//          Copyright Gavin Band 2008 - 2012.
// 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)

#ifndef METRO_REGRESSION_BINOMIALLOGISTIC_HPP
#define METRO_REGRESSION_BINOMIALLOGISTIC_HPP

#include <vector>
#include <memory>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"

namespace metro {
	namespace regression {
		/*
		* This class implements a log-likelihood for binomial logistic regression, allowing predictors
		* to take one of a finite set of values with associated probabilities. (A "missing data log-likelihood" ).
		* The outcome must be a two-column matrix containing positive integers; in the binomial formulation
		* in terms of 'successes/failures', the 1st column is the number of failure (baseline) outcomes
		* and the 2nd column the number of successful (non-baseline) outcomes.
		*/
		struct BinomialLogistic: public LogLikelihood
		{
		public:
			typedef regression::Design::Point Point ;
			typedef regression::Design::Vector Vector ;
			typedef regression::Design::RowVector RowVector ;
			typedef regression::Design::Matrix Matrix ;
			typedef Eigen::Block< Matrix > MatrixBlock ;
			typedef Eigen::Block< Matrix const > ConstMatrixBlock ;
			typedef boost::function< std::string( std::string const& predictor_name, std::string const& outcome_name ) > GetParameterName ;
		public:
			typedef std::auto_ptr< BinomialLogistic > UniquePtr ;
			static UniquePtr create( Design& ) ;
			static UniquePtr create( Design::UniquePtr ) ;
			static UniquePtr create( Design&, std::vector< metro::SampleRange > ) ;
			static UniquePtr create( Design::UniquePtr, std::vector< metro::SampleRange > ) ;
			BinomialLogistic( Design& ) ;
			BinomialLogistic( Design&, std::vector< metro::SampleRange > included_samples ) ;
			BinomialLogistic( Design::UniquePtr ) ;
			BinomialLogistic( Design::UniquePtr, std::vector< metro::SampleRange > included_samples ) ;
			~BinomialLogistic() ;

			regression::Design& design() const { return *m_design ; }
			void set_parameter_naming_scheme( GetParameterName ) ;
			std::string get_parameter_name( std::size_t i ) const ;

			int number_of_outcomes() const ;
			int number_of_parameters() const { return m_parameters.size() ; }
			IntegerMatrix identify_parameters() const ;

			void evaluate_at( Point const& parameters, int const numberOfDerivatives = 2 ) ;
			void evaluate( int const numberOfDerivatives = 2 ) ;

			Point const& parameters() const ;
			double get_value_of_function() const ;
			Vector get_value_of_first_derivative() const ;
			Matrix get_value_of_second_derivative() const ;

			std::string get_summary() const ;

		private:
			regression::Design* m_design ;
			bool m_design_owned ;
			GetParameterName m_get_parameter_name ;
			Point m_parameters ;
			std::vector< metro::SampleRange > m_included_samples ;
			std::vector< metro::SampleRange > m_evaluated_samples ;
			std::vector< metro::SampleRange > m_storage_samples ;
			int m_number_of_stored_samples ;
			int m_numberOfDerivativesComputed ;
			int m_numberOfCDLDerivativesComputed ;

			Matrix m_outcome_probabilities ;

			// Given outcome, likelihood function is of the form f(x^t theta) where
			// theta are the parameters (currently we don't handle the case e.g. normal
			// linear regression where there are additional parameters).
			// Therefore derivatives are of the form (Df)|(x^t theta).x^t
			// And D^2 f(x^t theta) . x^t

			Matrix m_hx ; // coefficient for each level x in complete data likelihood for each sample.
			Matrix m_normalisedDhx ; // coefficient of x in 1st derivative of mean function.
			Matrix m_normalisedDdhx ; // coefficient of x^t ⊗ x in 2nd derivative of mean function.
			Matrix m_f1 ; // temp storage used in likelihood and derivative computations

			Matrix m_first_derivative_terms ;
			double m_value_of_function ;
			Vector m_value_of_first_derivative ;
			Matrix m_value_of_second_derivative ;

		private:
			// Evaluate
			void evaluate_at_impl( Point const& parameters, std::vector< metro::SampleRange > const& included_samples, int const numberOfDerivatives ) ;

			// setup_storage(): this allocates space in:
			// m_hx, m_normalisedDhx, m_normalisedDdhx,
			// m_first_derivative_terms,
			// and defines m_storage_samples and m_number_of_stored_samples
			// appropriately for the ranges of samples passed in.
			void setup_storage( std::vector< metro::SampleRange > const& evaluation_samples ) ;

			// Compute complete data likelihood, and its derivatives, for each predictor level
			// and each sample
			virtual void compute_complete_data_likelihood_and_derivatives(
				Point const& parameters,
				Matrix* fx,
				Matrix* dfx,
				Matrix* ddfx,
				int const numberOfDerivatives
			) ;

			// Calculate matrix of probabilities of outcome per genotype, given the parameters.
			void calculate_outcome_probabilities( Vector const& parameters, Matrix const& phenotypes, Matrix* result ) const ;
			void compute_value_of_loglikelihood(
				Matrix const& hx,
				double* result
			) ;
			void compute_value_of_first_derivative(
				Matrix const& normalisedDhx,
				Matrix* result_terms,
				Vector* result
			) ;
			void compute_value_of_second_derivative(
				Matrix const& first_derivative_terms,
				Matrix const& normalisedDdhx,
				Matrix* result
			) ;
		} ;
	}
}

#endif


/*******************************************************************************
Name: Samuel Wenninger, Brandon Drumheller, and Malcolm Lorber
Date Created: 11-25-2015
Filename: proxy.cpp
Description: ATM program that is used to communicate with the Bank securely (see
             README for more information). Compile using the following command:
             g++ -std=c++11 atm.cpp -lboost_system -lboost_thread -lboost_regex
             -pthread -o atm.out
*******************************************************************************/

//Standard library
#include <iostream>
#include <string>
#include <fstream>
#include <algorithm>
#include <iomanip>
//Boost
#include <boost/asio.hpp>
//Program header files
#include "validate.h"
//Crypto++
#include "cryptopp/osrng.h"
#include "cryptopp/integer.h"
#include "cryptopp/nbtheory.h"
#include "cryptopp/dh.h"
#include "cryptopp/secblock.h"
#include "cryptopp/hex.h"
#include "cryptopp/filters.h"
#include "cryptopp/sha.h"
#include "cryptopp/aes.h"
#include "cryptopp/modes.h"
#include "cryptopp/rsa.h"
#include "cryptopp/base64.h"
#include "cryptopp/files.h"

using boost::asio::ip::tcp;
using namespace CryptoPP;
using namespace std;

/*******************************************************************************
 @DESC: Establish connection to the Bank, send the user inputted commands to the
        Bank (if valid), receive the response from the Bank, and display the
        response to the user. The user is able to continually enter commands,
        even after entering an invalid command.
 @ARGS: Port number used to connect to the proxy.
 @RTN:  EXIT_SUCCESS on success, EXIT_FAILURE on failure.
*******************************************************************************/
int main(int argc, char** argv) {
    try {

        Integer p("0x87A8E61DB4B6663CFFBBD19C651959998CEEF608660DD0F2\
                   5D2CEED4435E3B00E00DF8F1D61957D4FAF7DF4561B2AA30\
                   16C3D91134096FAA3BF4296D830E9A7C209E0C6497517ABD\
                   5A8A9D306BCF67ED91F9E6725B4758C022E0B1EF4275BF7B\
                   6C5BFC11D45F9088B941F54EB1E59BB8BC39A0BF12307F5C\
                   4FDB70C581B23F76B63ACAE1CAA6B7902D52526735488A0E\
                   F13C6D9A51BFA4AB3AD8347796524D8EF6A167B5A41825D9\
                   67E144E5140564251CCACB83E6B486F6B3CA3F7971506026\
                   C0B857F689962856DED4010ABD0BE621C3A3960A54E710C3\
                   75F26375D7014103A4B54330C198AF126116D2276E11715F\
                   693877FAD7EF09CADB094AE91E1A1597");
        Integer g("0x3FB32C9B73134D0B2E77506660EDBD484CA7B18F21EF2054\
                   07F4793A1A0BA12510DBC15077BE463FFF4FED4AAC0BB555\
                   BE3A6C1B0C6B47B1BC3773BF7E8C6F62901228F8C28CBB18\
                   A55AE31341000A650196F931C77A57F2DDF463E5E9EC144B\
                   777DE62AAAB8A8628AC376D282D6ED3864E67982428EBC83\
                   1D14348F6F2F9193B5045AF2767164E1DFC967C1FB3F2E55\
                   A4BD1BFFE83B9C80D052B985D182EA0ADB2A3B7313D3FE14\
                   C8484B1E052588B9B7D2BBD2DF016199ECD06E1557CD0915\
                   B3353BBB64E0EC377FD028370DF92B52C7891428CDC67EB6\
                   184B523D1DB246C32F63078490F00EF8D647D148D4795451\
                   5E2327CFEF98C582664B4C0F6CC41659");
        Integer q("0x8CF83642A709A097B447997640129DA299B1A47D1EB3750BA308B0FE64F5FBD3");

        if (argc != 2) {
            std::cerr << "ERROR: USAGE <port-to-proxy>" << std::endl;
            return EXIT_FAILURE;
        }
        const std::string ProxyPort = argv[1];
        const std::string host = "127.0.0.1";
        boost::asio::io_service io_service;
        tcp::resolver resolver(io_service);
        tcp::resolver::query query(tcp::v4(), host, ProxyPort);
        tcp::resolver::iterator iterator = resolver.resolve(query);
        tcp::socket s(io_service);
        boost::asio::connect(s, iterator);
        std::string request;

        //Handshake begins here
        //ATM RSA private key
        FileSource privfile("atmprivkey.txt", true, new Base64Decoder);
        //ATM RSA public key
        FileSource pubfile("atmpubkey.txt", true, new Base64Decoder);
        //Bank RSA public key
        FileSource bankpubfile("pubkey.txt", true, new Base64Decoder);

        //Load ATM RSA private key taken from the file into a RSA private key
        //primitive (i.e. make it usable to the code that follows).
        ByteQueue privbytes;
        privfile.TransferTo(privbytes);
        RSA::PrivateKey privkey;
        privkey.Load(privbytes);

        //Load ATM RSA public key taken from the file into a RSA private key
        //primitive (i.e. make it usable to the code that follows).
        ByteQueue pubbytes;
        pubfile.TransferTo(pubbytes);
        RSA::PublicKey pubkey;
        pubkey.Load(pubbytes);

        //Load Bank RSA public key taken from the file into a RSA private key
        //primitive (i.e. make it usable to the code that follows).
        ByteQueue bankpubbytes;
        bankpubfile.TransferTo(bankpubbytes);
        RSA::PublicKey bankpubkey;
        bankpubkey.Load(bankpubbytes);

        //Initialize the random number generator
        AutoSeededRandomPool rndA;
        boost::system::error_code EC;

        //Generate the challenge (random number) to send to the Bank
        byte challenge[128];
        rndA.GenerateBlock(challenge,128);
        std::string chaldisp;
        StringSource(challenge, 128, true,
                                    new HexEncoder(new StringSink(chaldisp)));
	//cout << "\nchal: " << chaldisp << endl;
        //Convert the challenge to an integer and encrypt it with the Bank's
        //public key.
        Integer m ((const byte*)challenge,128);
        Integer c = bankpubkey.ApplyFunction(m);

        //Convert the encrypted challenge back into a string.
        std::string challengeenc;
        size_t req = c.MinEncodedSize();
        challengeenc.resize(req);
        c.Encode((byte*)challengeenc.data(),challengeenc.size());

        //Base 64 encode the encrypted challenge.
        std::string challenge64;
        StringSource pubss(challengeenc, true, new Base64Encoder(new StringSink(challenge64)));
        challenge64.erase(std::remove(challenge64.begin(),challenge64.end(),'\n'),challenge64.end());

        //Send the encrypted challenge to the Bank.
        boost::asio::write(s, boost::asio::buffer(challenge64),
                                boost::asio::transfer_all(), EC);
        if ((boost::asio::error::eof == EC) ||
            (boost::asio::error::connection_reset == EC)) {
            std::cerr << "ERROR" << std::endl;
        }

        //If all has gone correctly, receive the same challenge sent to the
        //Bank, from the Bank. However, this time the challenge is encrypted
        //with the ATM's public key by the Bank.
        boost::asio::streambuf pubresponse;
        boost::asio::read_until(s, pubresponse, "\0");
        std::istream pub_response_stream(&pubresponse);
        std::string pubanswer;
        std::getline(pub_response_stream, pubanswer);

        //Decode the received encrypted challenge from Base 64 into non-Base 64
        //format.
        std::string pubdec;
        StringSource pubrss(pubanswer, true, new Base64Decoder(new StringSink(pubdec)));

        //Decrypt the encrypted challenge received from the Bank using the ATM's
        //RSA private key.
        Integer c2((const byte*)pubdec.data(),pubdec.size());
        Integer r = privkey.CalculateInverse(rndA, c2);
        std::string chalrecovered;
        size_t reqq = r.MinEncodedSize();
        chalrecovered.resize(reqq);
        r.Encode((byte *)chalrecovered.data(), chalrecovered.size());
        std::string chaldisp2;
        StringSource(chalrecovered, true,
                                    new HexEncoder(new StringSink(chaldisp2)));

        //Ensure that the challenge received from the Bank after decryption is
        //the same challenge that was originally generated, encrypted using the
        //Bank's RSA public key, and sent to the Bank.
        if(chaldisp != chaldisp2){
            throw runtime_error("Phony baloney");
        }

        //Begin the Diffie-Hellman key exchange.
        DH dhA;

        //Initialize the Diffie-Hellman parameters.
        dhA.AccessGroupParameters().Initialize(p, q, g);

        //Validate the initialized Diffie-Hellman parameters.
        if(!dhA.GetGroupParameters().ValidateGroup(rndA, 3))
            throw runtime_error("Failed to validate prime and generator");

        size_t count = 0;

        Integer v = ModularExponentiation(g, q, p);
        if(v != Integer::One())
            throw runtime_error("Failed to verify order of the subgroup");

        //Create the public and private Diffie-Hellman keys.
        SecByteBlock privA(dhA.PrivateKeyLength());
        SecByteBlock pubA(dhA.PublicKeyLength());
        dhA.GenerateKeyPair(rndA, privA, pubA);

        //Begin exchanging the Diffie-Hellman public keys. Send the ATM's
        //Diffie-Hellman public key to the Bank after Base 64 encoding it.
        std::string pubA64;
        StringSource ss(pubA.data(),pubA.size()+1,true,new Base64Encoder(new StringSink(pubA64)));
        pubA64.erase(std::remove(pubA64.begin(),pubA64.end(),'\n'), pubA64.end());
        boost::asio::write(s, boost::asio::buffer(pubA64),
                                boost::asio::transfer_all(), EC);
        if ((boost::asio::error::eof == EC) ||
            (boost::asio::error::connection_reset == EC)) {
            std::cerr << "ERROR" << std::endl;
        }
        //Receive the Bank's Diffie-Hellman public key.
        boost::asio::streambuf response;
        boost::asio::read_until(s, response, "\0");
        std::istream response_stream(&response);
        std::string answer;
        std::getline(response_stream, answer);

        //Send an ack to the Bank
        boost::asio::write(s, boost::asio::buffer("ack"),
                                boost::asio::transfer_all(), EC);
        if ((boost::asio::error::eof == EC) ||
            (boost::asio::error::connection_reset == EC)) {
            std::cerr << "ERROR" << std::endl;
        }

        //Base 64 decode the Bank's Diffie-Hellman public key.
        std::string pubBstr;
        StringSource ss2(answer, true, new Base64Decoder(new StringSink(pubBstr)));
        SecByteBlock pubB((byte*)pubBstr.data(),pubBstr.size());

        SecByteBlock sharedA(dhA.AgreedValueLength());

        //Ensure that a shared secret was established.
        if(!dhA.Agree(sharedA, privA, pubB))
            throw runtime_error("Failed to reach shared secret (1A)");

        //Integer a;
	//a.Decode(sharedA.BytePtr(), sharedA.SizeInBytes());
        //cout << "\nShared secret (A): " << std::hex << a << endl;

        //Begin using AES CFB
        int aesKeyLength = SHA256::DIGESTSIZE;
        int defBlockSize = AES::BLOCKSIZE;
        SecByteBlock key(SHA256::DIGESTSIZE);
        SHA256().CalculateDigest(key, sharedA, sharedA.size());
        byte iv1[AES::BLOCKSIZE];

        //Get the initial value from the Bank
        boost::asio::streambuf response2;
        boost::asio::read_until(s, response2, "\0");
        std::istream response_stream2(&response2);
        std::string answer2;
        std::getline(response_stream2, answer2);
        std::string iv1string;
        StringSource ssiv1(answer2,true,new Base64Decoder(new StringSink(iv1string)));
        if(iv1string.length()<16){
            throw std::exception();
        }
        for(int i=0;i<16;i++){
            iv1[i]=iv1string[i];
        }

        CFB_Mode<AES>::Encryption cfbEncryption(key, aesKeyLength, iv1);
        CFB_Mode<AES>::Decryption cfbDecryption(key, aesKeyLength, iv1);

        //The handshake has been completed and AES CFB is used from this point
        //onward.
	while (std::getline(std::cin, request)) {
            //Validate the user inputted command using regex (see "validate.h")
            if (!IsValidATMCommand(request)) {
                std::cerr << "INVALID COMMAND" << std::endl;
                continue;
            }
            //Setup login using ATM cards.
            if (request.find("login") == 0){
                int space = request.find(" ") +1;
                int account = 0;
                std::string accountStr = "";
                std::string user = request.substr(space, request.size() - space);
                std::ifstream cardFile(user + ".card");
                if(cardFile.is_open()){
                    cardFile >> account;
                    std::stringstream ss;
                    ss << account;
                    accountStr = ss.str();
                }
                else{
                    std::cerr << "Unable to read card file" << std::endl;
                }
                cardFile.close();
                //Replace "login <username>" with "login <account-num>" before
                //sending to the Bank.
                request = "login " + accountStr;
            }

            std::string encryptedRequest;
            StringSource es(request, true, new StreamTransformationFilter(cfbEncryption,new StringSink(encryptedRequest)));
            std::string encryptedRequest64;
            StringSource aesEncode(encryptedRequest,true,new Base64Encoder(new StringSink(encryptedRequest64)));
            encryptedRequest64.erase(std::remove(encryptedRequest64.begin(),encryptedRequest64.end(),'\n'), encryptedRequest64.end());

            boost::system::error_code EC;
            boost::asio::write(s, boost::asio::buffer(encryptedRequest64),
                                boost::asio::transfer_all(), EC);
            if ((boost::asio::error::eof == EC) ||
                              (boost::asio::error::connection_reset == EC)) {
                std::cerr << "ERROR" << std::endl;
            }
            //Read and display the Bank's response.
            boost::asio::streambuf response;
            boost::asio::read_until(s, response, "\0");
            std::istream response_stream(&response);
            std::string answer;
            std::getline(response_stream, answer);

            std::string data64dec;
            std::string sdata(answer);
            StringSource ss(sdata,true, new Base64Decoder(new StringSink(data64dec)));
            std::string decryptedRequest;
            StringSource ds(data64dec, true, new StreamTransformationFilter(cfbDecryption,new StringSink(decryptedRequest)));

            std::cout << /*answer*/ decryptedRequest << std::endl;
        }
    }
    //If anything goes wrong, the Bank will terminate its connection with the
    //ATM.
    catch (std::exception & e){
        std::cerr << "DISCONNECTED FROM BANK" << std::endl;
    }
    return EXIT_SUCCESS;
}