<commit_before>/*	libscratch - Multipurpose objective C++ library.

		Copyright (c) 2013 - 2016 Angelo Geels <spansjh@gmail.com>

		Permission is hereby granted, free of charge, to any person
		obtaining a copy of this software and associated documentation
		files (the "Software"), to deal in the Software without
		restriction, including without limitation the rights to use,
		copy, modify, merge, publish, distribute, sublicense, and/or sell
		copies of the Software, and to permit persons to whom the
		Software is furnished to do so, subject to the following
		conditions:

		The above copyright notice and this permission notice shall be
		included in all copies or substantial portions of the Software.

		THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
		EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
		OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
		NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
		HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
		WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
		FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
		OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#ifdef SCRATCH_IMPL
#include <cstdlib>
#include <cstring>
#endif

#ifndef SCRATCH_NO_THREADSAFE
#include "Mutex.hpp"
#endif

SCRATCH_NAMESPACE_BEGIN;

template<class Type>
class SCRATCH_EXPORT StackArray
{
public:
	Type** sa_pItems;
	int32_t sa_ctSlots;
	int32_t sa_ctUsed;
	bool sa_bOnlyPop;
#ifndef SCRATCH_NO_THREADSAFE
	Mutex sa_mutex;
#endif

public:
	StackArray();
	StackArray(const StackArray<Type> &copy); // Note: If this ever gets called, you're most likely writing bad code.
	StackArray<Type> &operator=(const StackArray<Type> &copy);
	~StackArray();

	/// Push to the beginning of the stack, return a reference to the newly made object
	Type& PushBegin();
	/// Push to the stack, return a reference to the newly made object
	Type& Push();
	/// Push a pointer to the stack
	void Push(Type* pObj);
	/// Pop top object from the stack
	Type* Pop();
	/// Pop a certain index from the stack
	Type* PopAt(int32_t iIndex);

	/// Pop all objects from the stack
	void PopAll();
	/// Clear all objects in the stack
	void Clear();

	/// Return how many objects there currently are in the stack
	int32_t Count();

	/// Find the index of the given object in the stack
	int32_t Find(const Type &obj);
	/// Find the index of the given pointer in the stack
	int32_t FindPointer(const Type* pObj);
	/// Find the index of the given condition in the stack
	template<typename Func>
	int32_t FindAny(Func f);

	/// Returns whether the given object is currently in the stack
	bool Contains(const Type &obj);
	/// Returns whether the given pointer is currently in the stack
	bool ContainsPointer(const Type* pObj);
	/// Returns whether the given condition is currently in the stack
	template<typename Func>
	bool ContainsAny(Func f);

	Type& operator[](int32_t iIndex);

private:
	void AllocateSlots(int32_t ctSlots);
};

#ifdef SCRATCH_IMPL

template<class Type>
StackArray<Type>::StackArray()
{
	sa_pItems = nullptr;
	sa_ctSlots = 0;
	sa_ctUsed = 0;
	sa_bOnlyPop = false;
	AllocateSlots(256);
}

template<class Type>
StackArray<Type>::StackArray(const StackArray<Type> &copy)
{
	operator=(copy);
}

template<class Type>
StackArray<Type> &StackArray<Type>::operator=(const StackArray<Type> &copy)
{
	sa_pItems = nullptr;
	sa_ctSlots = 0;
	sa_ctUsed = 0;
	sa_bOnlyPop = copy.sa_bOnlyPop;

	AllocateSlots(copy.sa_ctSlots);

	// copy meta information
	sa_ctUsed = copy.sa_ctUsed;

	// copy data to other slots
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// allocate memory for it
		sa_pItems[i] = new Type;

		// this should call the copy constructor
		*sa_pItems[i] = *copy.sa_pItems[i];
	}

	return *this;
}

template<class Type>
StackArray<Type>::~StackArray()
{
	if (sa_bOnlyPop) {
		PopAll();
	} else {
		Clear();
	}

	// free allocated memory for data
	if (sa_pItems != nullptr) {
		free(sa_pItems);
	}
}

template<class Type>
void StackArray<Type>::AllocateSlots(int32_t ctSlots)
{
	// allocate some memory
	sa_ctSlots += ctSlots;
	Type** pNewMem = (Type**)malloc(sizeof(Type*) * sa_ctSlots);

	// if we allocated memory before
	if (sa_pItems != nullptr) {
		// copy data to the new memory
		memcpy(pNewMem, sa_pItems, sizeof(Type*) * (sa_ctSlots - ctSlots));
		// free previously allocated memory
		free(sa_pItems);
	}

	// update the pointer
	sa_pItems = pNewMem;
}

/// Push to the beginning of the stack, return a reference to the newly made object
template<class Type>
Type& StackArray<Type>::PushBegin()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// PushBegin() in combination with sa_bOnlyPop will cause memory leaking if not manually Clear()'d
	ASSERT(!sa_bOnlyPop);

	// if we need more slots
	if (sa_ctUsed >= sa_ctSlots) {
		// allocate some more
		AllocateSlots(256);
	}

	// create the new object
	Type* tNewObject = new Type;

	// make some room
	for (int i = sa_ctUsed; i > 0; i--) {
		sa_pItems[i] = sa_pItems[i - 1];
	}

	// push it onto the beginning of the stack
	sa_pItems[0] = tNewObject;

	// increase iterator
	sa_ctUsed++;

	// return new object
	return *tNewObject;
}

/// Push to the stack, return a reference to the newly made object
template<class Type>
Type& StackArray<Type>::Push()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// Push() in combination with sa_bOnlyPop will cause memory leaking if not manually Clear()'d
	ASSERT(!sa_bOnlyPop);

	// if we need more slots
	if (sa_ctUsed >= sa_ctSlots) {
		// allocate some more
		AllocateSlots(256);
	}

	// create the new object
	Type* tNewObject = new Type;

	// push it onto the stack
	sa_pItems[sa_ctUsed] = tNewObject;

	// increase iterator
	sa_ctUsed++;

	// return new object
	return *tNewObject;
}

/// Push a pointer to the stack
template<class Type>
void StackArray<Type>::Push(Type* pObj)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// if we need more slots
	if (sa_ctUsed >= sa_ctSlots) {
		// allocate some more
		AllocateSlots(256);
	}

	// push it onto the stack
	sa_pItems[sa_ctUsed] = pObj;

	// increase iterator
	sa_ctUsed++;
}

/// Pop top object from the stack
template<class Type>
Type* StackArray<Type>::Pop()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	ASSERT(sa_ctUsed > 0);

	// decrease iterator
	sa_ctUsed--;

	// get the item on top of the stack
	Type* tObject = sa_pItems[sa_ctUsed];

	// set the remaining pointer to nullptr (just to be sure)
	sa_pItems[sa_ctUsed] = nullptr;

	// return the object
	return tObject;
}

/// Pop a certain index from the stack
template<class Type>
Type* StackArray<Type>::PopAt(int32_t iIndex)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	ASSERT(iIndex < sa_ctUsed);

	// decrease iterator
	sa_ctUsed--;

	// get the item in the stack
	Type* tObject = sa_pItems[iIndex];

	// move pointers at the right one point to the left
	for (int32_t i = iIndex; i < sa_ctUsed; i++) {
		// set pointer to next one in stack
		sa_pItems[i] = sa_pItems[i + 1];
	}

	// set last pointer to nullptr (just in case)
	sa_pItems[sa_ctUsed] = nullptr;

	// return the object
	return tObject;
}

/// Pop all objects from the stack
template<class Type>
void StackArray<Type>::PopAll()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// set remaining pointer to nullptr (just to be sure)
		sa_pItems[i] = nullptr;
	}

	// reset iterator to 0
	sa_ctUsed = 0;
}

/// Clear all objects in the stack
template<class Type>
void StackArray<Type>::Clear()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// delete it
		delete sa_pItems[i];
		// set remaining pointer to nullptr (just to be sure)
		sa_pItems[i] = nullptr;
	}

	// reset iterator to 0
	sa_ctUsed = 0;
}

/// Return how many objects there currently are in the stack
template<class Type>
int32_t StackArray<Type>::Count()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif
	return sa_ctUsed;
}

/// Find the index of the given object in the stack
template<class Type>
int32_t StackArray<Type>::Find(const Type &obj)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// test if it's the given one
		if (obj == *sa_pItems[i]) {
			return i;
		}
	}

	return -1;
}

/// Find the index of the given pointer in the stack
template<class Type>
int32_t StackArray<Type>::FindPointer(const Type* pObj)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// test if it's the given one
		if (pObj == sa_pItems[i]) {
			return i;
		}
	}

	return -1;
}

/// Find the index of the given condition in the stack
template<class Type>
template<typename Func>
int32_t StackArray<Type>::FindAny(Func f)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// test with function
		if (f(*sa_pItems[i])) {
			return i;
		}
	}
	return -1;
}

/// Returns whether the given object is currently in the stack
template<class Type>
bool StackArray<Type>::Contains(const Type &obj)
{
	return Find(obj) != -1;
}

/// Returns whether the given pointer is currently in the stack
template<class Type>
bool StackArray<Type>::ContainsPointer(const Type* pObj)
{
	return FindPointer(pObj) != -1;
}

/// Returns whether the given condition exists on any of the objects currently in the stack
template<class Type>
template<typename Func>
bool StackArray<Type>::ContainsAny(Func f)
{
	return FindAny(f) != -1;
}

template<class Type>
Type& StackArray<Type>::operator[](int32_t iIndex)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif
	ASSERT(iIndex >= 0 && iIndex < sa_ctUsed);
	return *sa_pItems[iIndex];
}

#endif

SCRATCH_NAMESPACE_END;
<commit_msg>Always allow implementation of stack array<commit_after>/*	libscratch - Multipurpose objective C++ library.

		Copyright (c) 2013 - 2016 Angelo Geels <spansjh@gmail.com>

		Permission is hereby granted, free of charge, to any person
		obtaining a copy of this software and associated documentation
		files (the "Software"), to deal in the Software without
		restriction, including without limitation the rights to use,
		copy, modify, merge, publish, distribute, sublicense, and/or sell
		copies of the Software, and to permit persons to whom the
		Software is furnished to do so, subject to the following
		conditions:

		The above copyright notice and this permission notice shall be
		included in all copies or substantial portions of the Software.

		THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
		EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
		OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
		NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
		HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
		WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
		FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
		OTHER DEALINGS IN THE SOFTWARE.
*/

#pragma once

#include <cstdlib>
#include <cstring>

#ifndef SCRATCH_NO_THREADSAFE
#include "Mutex.hpp"
#endif

SCRATCH_NAMESPACE_BEGIN;

template<class Type>
class SCRATCH_EXPORT StackArray
{
public:
	Type** sa_pItems;
	int32_t sa_ctSlots;
	int32_t sa_ctUsed;
	bool sa_bOnlyPop;
#ifndef SCRATCH_NO_THREADSAFE
	Mutex sa_mutex;
#endif

public:
	StackArray();
	StackArray(const StackArray<Type> &copy); // Note: If this ever gets called, you're most likely writing bad code.
	StackArray<Type> &operator=(const StackArray<Type> &copy);
	~StackArray();

	/// Push to the beginning of the stack, return a reference to the newly made object
	Type& PushBegin();
	/// Push to the stack, return a reference to the newly made object
	Type& Push();
	/// Push a pointer to the stack
	void Push(Type* pObj);
	/// Pop top object from the stack
	Type* Pop();
	/// Pop a certain index from the stack
	Type* PopAt(int32_t iIndex);

	/// Pop all objects from the stack
	void PopAll();
	/// Clear all objects in the stack
	void Clear();

	/// Return how many objects there currently are in the stack
	int32_t Count();

	/// Find the index of the given object in the stack
	int32_t Find(const Type &obj);
	/// Find the index of the given pointer in the stack
	int32_t FindPointer(const Type* pObj);
	/// Find the index of the given condition in the stack
	template<typename Func>
	int32_t FindAny(Func f);

	/// Returns whether the given object is currently in the stack
	bool Contains(const Type &obj);
	/// Returns whether the given pointer is currently in the stack
	bool ContainsPointer(const Type* pObj);
	/// Returns whether the given condition is currently in the stack
	template<typename Func>
	bool ContainsAny(Func f);

	Type& operator[](int32_t iIndex);

private:
	void AllocateSlots(int32_t ctSlots);
};

template<class Type>
StackArray<Type>::StackArray()
{
	sa_pItems = nullptr;
	sa_ctSlots = 0;
	sa_ctUsed = 0;
	sa_bOnlyPop = false;
	AllocateSlots(256);
}

template<class Type>
StackArray<Type>::StackArray(const StackArray<Type> &copy)
{
	operator=(copy);
}

template<class Type>
StackArray<Type> &StackArray<Type>::operator=(const StackArray<Type> &copy)
{
	sa_pItems = nullptr;
	sa_ctSlots = 0;
	sa_ctUsed = 0;
	sa_bOnlyPop = copy.sa_bOnlyPop;

	AllocateSlots(copy.sa_ctSlots);

	// copy meta information
	sa_ctUsed = copy.sa_ctUsed;

	// copy data to other slots
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// allocate memory for it
		sa_pItems[i] = new Type;

		// this should call the copy constructor
		*sa_pItems[i] = *copy.sa_pItems[i];
	}

	return *this;
}

template<class Type>
StackArray<Type>::~StackArray()
{
	if (sa_bOnlyPop) {
		PopAll();
	} else {
		Clear();
	}

	// free allocated memory for data
	if (sa_pItems != nullptr) {
		free(sa_pItems);
	}
}

template<class Type>
void StackArray<Type>::AllocateSlots(int32_t ctSlots)
{
	// allocate some memory
	sa_ctSlots += ctSlots;
	Type** pNewMem = (Type**)malloc(sizeof(Type*) * sa_ctSlots);

	// if we allocated memory before
	if (sa_pItems != nullptr) {
		// copy data to the new memory
		memcpy(pNewMem, sa_pItems, sizeof(Type*) * (sa_ctSlots - ctSlots));
		// free previously allocated memory
		free(sa_pItems);
	}

	// update the pointer
	sa_pItems = pNewMem;
}

/// Push to the beginning of the stack, return a reference to the newly made object
template<class Type>
Type& StackArray<Type>::PushBegin()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// PushBegin() in combination with sa_bOnlyPop will cause memory leaking if not manually Clear()'d
	ASSERT(!sa_bOnlyPop);

	// if we need more slots
	if (sa_ctUsed >= sa_ctSlots) {
		// allocate some more
		AllocateSlots(256);
	}

	// create the new object
	Type* tNewObject = new Type;

	// make some room
	for (int i = sa_ctUsed; i > 0; i--) {
		sa_pItems[i] = sa_pItems[i - 1];
	}

	// push it onto the beginning of the stack
	sa_pItems[0] = tNewObject;

	// increase iterator
	sa_ctUsed++;

	// return new object
	return *tNewObject;
}

/// Push to the stack, return a reference to the newly made object
template<class Type>
Type& StackArray<Type>::Push()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// Push() in combination with sa_bOnlyPop will cause memory leaking if not manually Clear()'d
	ASSERT(!sa_bOnlyPop);

	// if we need more slots
	if (sa_ctUsed >= sa_ctSlots) {
		// allocate some more
		AllocateSlots(256);
	}

	// create the new object
	Type* tNewObject = new Type;

	// push it onto the stack
	sa_pItems[sa_ctUsed] = tNewObject;

	// increase iterator
	sa_ctUsed++;

	// return new object
	return *tNewObject;
}

/// Push a pointer to the stack
template<class Type>
void StackArray<Type>::Push(Type* pObj)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// if we need more slots
	if (sa_ctUsed >= sa_ctSlots) {
		// allocate some more
		AllocateSlots(256);
	}

	// push it onto the stack
	sa_pItems[sa_ctUsed] = pObj;

	// increase iterator
	sa_ctUsed++;
}

/// Pop top object from the stack
template<class Type>
Type* StackArray<Type>::Pop()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	ASSERT(sa_ctUsed > 0);

	// decrease iterator
	sa_ctUsed--;

	// get the item on top of the stack
	Type* tObject = sa_pItems[sa_ctUsed];

	// set the remaining pointer to nullptr (just to be sure)
	sa_pItems[sa_ctUsed] = nullptr;

	// return the object
	return tObject;
}

/// Pop a certain index from the stack
template<class Type>
Type* StackArray<Type>::PopAt(int32_t iIndex)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	ASSERT(iIndex < sa_ctUsed);

	// decrease iterator
	sa_ctUsed--;

	// get the item in the stack
	Type* tObject = sa_pItems[iIndex];

	// move pointers at the right one point to the left
	for (int32_t i = iIndex; i < sa_ctUsed; i++) {
		// set pointer to next one in stack
		sa_pItems[i] = sa_pItems[i + 1];
	}

	// set last pointer to nullptr (just in case)
	sa_pItems[sa_ctUsed] = nullptr;

	// return the object
	return tObject;
}

/// Pop all objects from the stack
template<class Type>
void StackArray<Type>::PopAll()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// set remaining pointer to nullptr (just to be sure)
		sa_pItems[i] = nullptr;
	}

	// reset iterator to 0
	sa_ctUsed = 0;
}

/// Clear all objects in the stack
template<class Type>
void StackArray<Type>::Clear()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// delete it
		delete sa_pItems[i];
		// set remaining pointer to nullptr (just to be sure)
		sa_pItems[i] = nullptr;
	}

	// reset iterator to 0
	sa_ctUsed = 0;
}

/// Return how many objects there currently are in the stack
template<class Type>
int32_t StackArray<Type>::Count()
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif
	return sa_ctUsed;
}

/// Find the index of the given object in the stack
template<class Type>
int32_t StackArray<Type>::Find(const Type &obj)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// test if it's the given one
		if (obj == *sa_pItems[i]) {
			return i;
		}
	}

	return -1;
}

/// Find the index of the given pointer in the stack
template<class Type>
int32_t StackArray<Type>::FindPointer(const Type* pObj)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// test if it's the given one
		if (pObj == sa_pItems[i]) {
			return i;
		}
	}

	return -1;
}

/// Find the index of the given condition in the stack
template<class Type>
template<typename Func>
int32_t StackArray<Type>::FindAny(Func f)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif

	// for every object
	for (int32_t i = 0; i < sa_ctUsed; i++) {
		// test with function
		if (f(*sa_pItems[i])) {
			return i;
		}
	}
	return -1;
}

/// Returns whether the given object is currently in the stack
template<class Type>
bool StackArray<Type>::Contains(const Type &obj)
{
	return Find(obj) != -1;
}

/// Returns whether the given pointer is currently in the stack
template<class Type>
bool StackArray<Type>::ContainsPointer(const Type* pObj)
{
	return FindPointer(pObj) != -1;
}

/// Returns whether the given condition exists on any of the objects currently in the stack
template<class Type>
template<typename Func>
bool StackArray<Type>::ContainsAny(Func f)
{
	return FindAny(f) != -1;
}

template<class Type>
Type& StackArray<Type>::operator[](int32_t iIndex)
{
#ifndef SCRATCH_NO_THREADSAFE
	MutexWait wait(sa_mutex);
#endif
	ASSERT(iIndex >= 0 && iIndex < sa_ctUsed);
	return *sa_pItems[iIndex];
}

SCRATCH_NAMESPACE_END;
<|endoftext|>


<commit_before>#include <map>
#include <vector>
#include <thread>
#include <mutex>

#include <assert.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>

#ifdef WIN32
	#include <winsock2.h>
	#include <ws2tcpip.h>
#else // WIN32
	#include <sys/socket.h>
	#include <netinet/in.h>
	#include <netinet/tcp.h>
	#include <netdb.h>
	#include <fcntl.h>
#endif // !WIN32

#define BITCOIN_UA_LENGTH 24
#define BITCOIN_UA {'/', 'R', 'e', 'l', 'a', 'y', 'N', 'e', 't', 'w', 'o', 'r', 'k', 'C', 'l', 'i', 'e', 'n', 't', ':', '4', '2', '/', '\0'}

#include "crypto/sha2.h"
#include "flaggedarrayset.h"
#include "relayprocess.h"
#include "utils.h"
#include "p2pclient.h"

/***********************************************
 **** Relay network client processing class ****
 ***********************************************/
class RelayNetworkClient : public OutboundPersistentConnection {
private:
	RELAY_DECLARE_CLASS_VARS

	const std::function<void (std::vector<unsigned char>&)> provide_block;
	const std::function<void (std::shared_ptr<std::vector<unsigned char> >&)> provide_transaction;
	const std::function<void (void)> on_connected;

	std::atomic_bool connected;

	RelayNodeCompressor compressor;

public:
	RelayNetworkClient(const char* serverHostIn,
						const std::function<void (std::vector<unsigned char>&)>& provide_block_in,
						const std::function<void (std::shared_ptr<std::vector<unsigned char> >&)>& provide_transaction_in,
						const std::function<void (void)>& on_connected_in)
			: OutboundPersistentConnection(serverHostIn, 8336), RELAY_DECLARE_CONSTRUCTOR_EXTENDS,
			provide_block(provide_block_in), provide_transaction(provide_transaction_in), on_connected(on_connected_in),
			connected(false), compressor(false) {
		construction_done();
	}

private:
	void on_disconnect() {
		connected = false;
	}

	void net_process(const std::function<void(const char*)>& disconnect) {
		compressor.reset();

		relay_msg_header version_header = { RELAY_MAGIC_BYTES, VERSION_TYPE, htonl(strlen(VERSION_STRING)) };
		maybe_do_send_bytes((char*)&version_header, sizeof(version_header));
		maybe_do_send_bytes(VERSION_STRING, strlen(VERSION_STRING));

		connected = true;

		while (true) {
			relay_msg_header header;
			if (read_all((char*)&header, 4*3) != 4*3)
				return disconnect("failed to read message header");

			if (header.magic != RELAY_MAGIC_BYTES)
				return disconnect("invalid magic bytes");

			uint32_t message_size = ntohl(header.length);

			if (message_size > 1000000)
				return disconnect("got message too large");

			if (header.type == VERSION_TYPE) {
				char data[message_size];
				if (read_all(data, message_size) < (int64_t)(message_size))
					return disconnect("failed to read version message");

				if (strncmp(VERSION_STRING, data, std::min(sizeof(VERSION_STRING), size_t(message_size))))
					return disconnect("unknown version string");
				else {
					printf("Connected to relay node with protocol version %s\n", VERSION_STRING);
					on_connected();
				}
			} else if (header.type == MAX_VERSION_TYPE) {
				char data[message_size];
				if (read_all(data, message_size) < (int64_t)(message_size))
					return disconnect("failed to read max_version string");

				if (strncmp(VERSION_STRING, data, std::min(sizeof(VERSION_STRING), size_t(message_size))))
					printf("Relay network is using a later version (PLEASE UPGRADE)\n");
				else
					return disconnect("got MAX_VERSION of same version as us");
			} else if (header.type == BLOCK_TYPE) {
				std::function<ssize_t(char*, size_t)> do_read = [&](char* buf, size_t count) { return this->read_all(buf, count); };
				auto res = compressor.decompress_relay_block(do_read, message_size, false);
				if (std::get<2>(res))
					return disconnect(std::get<2>(res));

				provide_block(*std::get<1>(res));

				auto fullhash = *std::get<3>(res).get();
				struct tm tm;
				time_t now = time(NULL);
				gmtime_r(&now, &tm);
				printf("[%d-%02d-%02d %02d:%02d:%02d+00] ", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec);
				for (unsigned int i = 0; i < fullhash.size(); i++)
					printf("%02x", fullhash[fullhash.size() - i - 1]);
				printf(" recv'd, size %lu with %u bytes on the wire\n", (unsigned long)std::get<1>(res)->size() - sizeof(bitcoin_msg_header), std::get<0>(res));
			} else if (header.type == END_BLOCK_TYPE) {
			} else if (header.type == TRANSACTION_TYPE) {
				if (!compressor.maybe_recv_tx_of_size(message_size, true))
					return disconnect("got freely relayed transaction too large");

				auto tx = std::make_shared<std::vector<unsigned char> > (message_size);
				if (read_all((char*)&(*tx)[0], message_size) < (int64_t)(message_size))
					return disconnect("failed to read loose transaction data");

				compressor.recv_tx(tx);
				provide_transaction(tx);
				printf("Received transaction of size %u from relay server\n", message_size);
			} else
				return disconnect("got unknown message type");
		}
	}

public:
	void receive_transaction(const std::shared_ptr<std::vector<unsigned char> >& tx, int token, bool send_oob) {
		if (!connected)
			return;

		std::shared_ptr<std::vector<unsigned char> > msgptr;
		if (send_oob)
			msgptr = compressor.tx_to_msg(tx, true);
		else
			msgptr = compressor.get_relay_transaction(tx);
		if (!msgptr.use_count())
			return;

		auto& msg = *msgptr.get();

		maybe_do_send_bytes((char*)&msg[0], msg.size(), token);
		printf("Sent transaction of size %lu%s to relay server\n", (unsigned long)tx->size(), send_oob ? " (out-of-band)" : "");
	}

	void receive_block(const std::vector<unsigned char>& block, int token) {
		if (!connected)
			return;

		std::vector<unsigned char> fullhash(32);
		getblockhash(fullhash, block, sizeof(struct bitcoin_msg_header));

		auto tuple = compressor.maybe_compress_block(fullhash, block, false);
		if (std::get<1>(tuple)) {
			printf("Failed to process block from bitcoind (%s)\n", std::get<1>(tuple));
			return;
		}
		auto compressed_block = std::get<0>(tuple);

		maybe_do_send_bytes((char*)&(*compressed_block)[0], compressed_block->size(), token);
		struct relay_msg_header header = { RELAY_MAGIC_BYTES, END_BLOCK_TYPE, 0 };
		maybe_do_send_bytes((char*)&header, sizeof(header), token);

		print_hash(&fullhash[0]);
		printf(" sent, size %lu with %lu bytes on the wire\n", (unsigned long)block.size(), (unsigned long)compressed_block->size());
	}
};

class P2PClient : public P2PRelayer {
public:
	P2PClient(const char* serverHostIn, uint16_t serverPortIn,
				const std::function<void (std::vector<unsigned char>&, const std::chrono::system_clock::time_point&)>& provide_block_in,
				const std::function<void (std::shared_ptr<std::vector<unsigned char> >&)>& provide_transaction_in,
				const std::function<void (void)>& mempools_done_in) :
			P2PRelayer(serverHostIn, serverPortIn, provide_block_in, provide_transaction_in, NULL, mempools_done_in, NULL, true)
		{ construction_done(); }

private:
	std::vector<unsigned char> generate_version() {
		struct bitcoin_version_with_header version_msg;
		version_msg.version.start.timestamp = htole64(time(0));
		version_msg.version.start.user_agent_length = BITCOIN_UA_LENGTH; // Work around apparent gcc bug
		return std::vector<unsigned char>((unsigned char*)&version_msg, (unsigned char*)&version_msg + sizeof(version_msg));
	}
};

#define HOSTNAMES_TO_TEST 20
#define CONNECT_TESTS 20
std::chrono::milliseconds connect_durations[HOSTNAMES_TO_TEST];
void test_node(int node) {
	const char* relay = "public.%02d.relay.mattcorallo.com";
	char host[strlen(relay)];
	sprintf(host, relay, node);
	sockaddr_in6 addr;
	if (!lookup_address(host, &addr) ||
			(addr.sin6_addr.s6_addr[15] == 0 && addr.sin6_addr.s6_addr[14] == 0 && addr.sin6_addr.s6_addr[13] == 0 && addr.sin6_addr.s6_addr[12] == 0)) {
		connect_durations[node] = std::chrono::milliseconds::max();
		return;
	}

	addr.sin6_port = htons(8336);

	auto start = std::chrono::steady_clock::now();
	for (int i = 0; i < CONNECT_TESTS; i++) {
		int sock = socket(AF_INET6, SOCK_STREAM, 0);
		ALWAYS_ASSERT(sock > 0);

		int v6only = 0;
		setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, (char *)&v6only, sizeof(v6only));

		connect(sock, (struct sockaddr*)&addr, sizeof(addr));
		close(sock);
	}
	connect_durations[node] = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start);
}

int main(int argc, char** argv) {
	bool validPort = false;
	try { std::stoul(argv[2]); validPort = true; } catch (std::exception& e) {}
	if ((argc != 3 && argc != 4) || !validPort) {
		printf("USAGE: %s BITCOIND_ADDRESS BITCOIND_PORT [ server ]\n", argv[0]);
		printf("Relay server is automatically selected by pinging available servers, unless one is specified\n");
		return -1;
	}

#ifdef WIN32
	WSADATA wsaData;
	if (WSAStartup(MAKEWORD(2,2), &wsaData))
		return -1;
#endif

	const char* relay = "public.%02d.relay.mattcorallo.com";
	char host[std::max(argc == 3 ? 0 : strlen(argv[3]), strlen(relay))];
	if (argc == 3) {
		while (true) {
			std::list<std::thread> threads;
			for (int i = 0; i < HOSTNAMES_TO_TEST; i++)
				threads.emplace_back(test_node, i);
			for (int i = 0; i < HOSTNAMES_TO_TEST; i++) {
				threads.front().join();
				threads.pop_front();
			}

			int min = -1; std::chrono::milliseconds min_duration(std::chrono::milliseconds::max());
			for (int i = 0; i < HOSTNAMES_TO_TEST; i++) {
				if (connect_durations[i] != std::chrono::milliseconds::max()) {
					std::string aka;
					sprintf(host, relay, i);
					printf("Server %d (%s) took %lld ms to respond %d times.\n", i, lookup_cname(host, aka) ? aka.c_str() : "", (long long int)connect_durations[i].count(), CONNECT_TESTS);
				}
				if (connect_durations[i] < min_duration) {
					min_duration = connect_durations[i];
					min = i;
				}
			}

			std::this_thread::sleep_for(std::chrono::seconds(10)); // Wait for server to open up our slot again
			if (min == -1) {
				printf("No servers responded\n");
				continue;
			}

			sprintf(host, relay, min);
			break;
		}
	} else
		memcpy(host, argv[3], strlen(argv[3]) + 1);
	printf("Using server %s\n", host);

	std::atomic_int relayToken(0);
	RelayNetworkClient* relayClient;
	P2PClient p2p(argv[1], std::stoul(argv[2]),
					[&](std::vector<unsigned char>& bytes, const std::chrono::system_clock::time_point&) { relayClient->receive_block(bytes, relayToken); },
					[&](std::shared_ptr<std::vector<unsigned char> >& bytes) {
						relayClient->receive_transaction(bytes, relayToken, !p2p.maybe_supports_mempool());
					},
					[&]() {
						relayClient->release_send_mutex(relayToken);
						relayToken = 0;
						if (!p2p.maybe_supports_mempool())
							p2p.regularly_request_mempool_and_dont_fetch_loose_txn = false;
					});
	relayClient = new RelayNetworkClient(host,
										[&](std::vector<unsigned char>& bytes) { p2p.receive_block(bytes); },
										[&](std::shared_ptr<std::vector<unsigned char> >& bytes) {
											p2p.receive_transaction(bytes);
											if (!p2p.maybe_supports_mempool())
												relayClient->receive_transaction(bytes, relayToken, false);
										},
										[&]() {
											relayToken = relayClient->get_send_mutex();
											relayClient->do_throttle_outbound(relayToken);
											p2p.request_mempool();
										});

	while (true) { sleep(1000); }
}
<commit_msg>Fix print ordering when send/recv'ing txn in client<commit_after>#include <map>
#include <vector>
#include <thread>
#include <mutex>

#include <assert.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>

#ifdef WIN32
	#include <winsock2.h>
	#include <ws2tcpip.h>
#else // WIN32
	#include <sys/socket.h>
	#include <netinet/in.h>
	#include <netinet/tcp.h>
	#include <netdb.h>
	#include <fcntl.h>
#endif // !WIN32

#define BITCOIN_UA_LENGTH 24
#define BITCOIN_UA {'/', 'R', 'e', 'l', 'a', 'y', 'N', 'e', 't', 'w', 'o', 'r', 'k', 'C', 'l', 'i', 'e', 'n', 't', ':', '4', '2', '/', '\0'}

#include "crypto/sha2.h"
#include "flaggedarrayset.h"
#include "relayprocess.h"
#include "utils.h"
#include "p2pclient.h"

/***********************************************
 **** Relay network client processing class ****
 ***********************************************/
class RelayNetworkClient : public OutboundPersistentConnection {
private:
	RELAY_DECLARE_CLASS_VARS

	const std::function<void (std::vector<unsigned char>&)> provide_block;
	const std::function<void (std::shared_ptr<std::vector<unsigned char> >&)> provide_transaction;
	const std::function<void (void)> on_connected;

	std::atomic_bool connected;

	RelayNodeCompressor compressor;

public:
	RelayNetworkClient(const char* serverHostIn,
						const std::function<void (std::vector<unsigned char>&)>& provide_block_in,
						const std::function<void (std::shared_ptr<std::vector<unsigned char> >&)>& provide_transaction_in,
						const std::function<void (void)>& on_connected_in)
			: OutboundPersistentConnection(serverHostIn, 8336), RELAY_DECLARE_CONSTRUCTOR_EXTENDS,
			provide_block(provide_block_in), provide_transaction(provide_transaction_in), on_connected(on_connected_in),
			connected(false), compressor(false) {
		construction_done();
	}

private:
	void on_disconnect() {
		connected = false;
	}

	void net_process(const std::function<void(const char*)>& disconnect) {
		compressor.reset();

		relay_msg_header version_header = { RELAY_MAGIC_BYTES, VERSION_TYPE, htonl(strlen(VERSION_STRING)) };
		maybe_do_send_bytes((char*)&version_header, sizeof(version_header));
		maybe_do_send_bytes(VERSION_STRING, strlen(VERSION_STRING));

		connected = true;

		while (true) {
			relay_msg_header header;
			if (read_all((char*)&header, 4*3) != 4*3)
				return disconnect("failed to read message header");

			if (header.magic != RELAY_MAGIC_BYTES)
				return disconnect("invalid magic bytes");

			uint32_t message_size = ntohl(header.length);

			if (message_size > 1000000)
				return disconnect("got message too large");

			if (header.type == VERSION_TYPE) {
				char data[message_size];
				if (read_all(data, message_size) < (int64_t)(message_size))
					return disconnect("failed to read version message");

				if (strncmp(VERSION_STRING, data, std::min(sizeof(VERSION_STRING), size_t(message_size))))
					return disconnect("unknown version string");
				else {
					printf("Connected to relay node with protocol version %s\n", VERSION_STRING);
					on_connected();
				}
			} else if (header.type == MAX_VERSION_TYPE) {
				char data[message_size];
				if (read_all(data, message_size) < (int64_t)(message_size))
					return disconnect("failed to read max_version string");

				if (strncmp(VERSION_STRING, data, std::min(sizeof(VERSION_STRING), size_t(message_size))))
					printf("Relay network is using a later version (PLEASE UPGRADE)\n");
				else
					return disconnect("got MAX_VERSION of same version as us");
			} else if (header.type == BLOCK_TYPE) {
				std::function<ssize_t(char*, size_t)> do_read = [&](char* buf, size_t count) { return this->read_all(buf, count); };
				auto res = compressor.decompress_relay_block(do_read, message_size, false);
				if (std::get<2>(res))
					return disconnect(std::get<2>(res));

				provide_block(*std::get<1>(res));

				auto fullhash = *std::get<3>(res).get();
				struct tm tm;
				time_t now = time(NULL);
				gmtime_r(&now, &tm);
				printf("[%d-%02d-%02d %02d:%02d:%02d+00] ", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec);
				for (unsigned int i = 0; i < fullhash.size(); i++)
					printf("%02x", fullhash[fullhash.size() - i - 1]);
				printf(" recv'd, size %lu with %u bytes on the wire\n", (unsigned long)std::get<1>(res)->size() - sizeof(bitcoin_msg_header), std::get<0>(res));
			} else if (header.type == END_BLOCK_TYPE) {
			} else if (header.type == TRANSACTION_TYPE) {
				if (!compressor.maybe_recv_tx_of_size(message_size, true))
					return disconnect("got freely relayed transaction too large");

				auto tx = std::make_shared<std::vector<unsigned char> > (message_size);
				if (read_all((char*)&(*tx)[0], message_size) < (int64_t)(message_size))
					return disconnect("failed to read loose transaction data");

				printf("Received transaction of size %u from relay server\n", message_size);

				compressor.recv_tx(tx);
				provide_transaction(tx);
			} else
				return disconnect("got unknown message type");
		}
	}

public:
	void receive_transaction(const std::shared_ptr<std::vector<unsigned char> >& tx, int token, bool send_oob) {
		if (!connected)
			return;

		std::shared_ptr<std::vector<unsigned char> > msgptr;
		if (send_oob)
			msgptr = compressor.tx_to_msg(tx, true);
		else
			msgptr = compressor.get_relay_transaction(tx);
		if (!msgptr.use_count())
			return;

		auto& msg = *msgptr.get();

		maybe_do_send_bytes((char*)&msg[0], msg.size(), token);
		printf("Sent transaction of size %lu%s to relay server\n", (unsigned long)tx->size(), send_oob ? " (out-of-band)" : "");
	}

	void receive_block(const std::vector<unsigned char>& block, int token) {
		if (!connected)
			return;

		std::vector<unsigned char> fullhash(32);
		getblockhash(fullhash, block, sizeof(struct bitcoin_msg_header));

		auto tuple = compressor.maybe_compress_block(fullhash, block, false);
		if (std::get<1>(tuple)) {
			printf("Failed to process block from bitcoind (%s)\n", std::get<1>(tuple));
			return;
		}
		auto compressed_block = std::get<0>(tuple);

		maybe_do_send_bytes((char*)&(*compressed_block)[0], compressed_block->size(), token);
		struct relay_msg_header header = { RELAY_MAGIC_BYTES, END_BLOCK_TYPE, 0 };
		maybe_do_send_bytes((char*)&header, sizeof(header), token);

		print_hash(&fullhash[0]);
		printf(" sent, size %lu with %lu bytes on the wire\n", (unsigned long)block.size(), (unsigned long)compressed_block->size());
	}
};

class P2PClient : public P2PRelayer {
public:
	P2PClient(const char* serverHostIn, uint16_t serverPortIn,
				const std::function<void (std::vector<unsigned char>&, const std::chrono::system_clock::time_point&)>& provide_block_in,
				const std::function<void (std::shared_ptr<std::vector<unsigned char> >&)>& provide_transaction_in,
				const std::function<void (void)>& mempools_done_in) :
			P2PRelayer(serverHostIn, serverPortIn, provide_block_in, provide_transaction_in, NULL, mempools_done_in, NULL, true)
		{ construction_done(); }

private:
	std::vector<unsigned char> generate_version() {
		struct bitcoin_version_with_header version_msg;
		version_msg.version.start.timestamp = htole64(time(0));
		version_msg.version.start.user_agent_length = BITCOIN_UA_LENGTH; // Work around apparent gcc bug
		return std::vector<unsigned char>((unsigned char*)&version_msg, (unsigned char*)&version_msg + sizeof(version_msg));
	}
};

#define HOSTNAMES_TO_TEST 20
#define CONNECT_TESTS 20
std::chrono::milliseconds connect_durations[HOSTNAMES_TO_TEST];
void test_node(int node) {
	const char* relay = "public.%02d.relay.mattcorallo.com";
	char host[strlen(relay)];
	sprintf(host, relay, node);
	sockaddr_in6 addr;
	if (!lookup_address(host, &addr) ||
			(addr.sin6_addr.s6_addr[15] == 0 && addr.sin6_addr.s6_addr[14] == 0 && addr.sin6_addr.s6_addr[13] == 0 && addr.sin6_addr.s6_addr[12] == 0)) {
		connect_durations[node] = std::chrono::milliseconds::max();
		return;
	}

	addr.sin6_port = htons(8336);

	auto start = std::chrono::steady_clock::now();
	for (int i = 0; i < CONNECT_TESTS; i++) {
		int sock = socket(AF_INET6, SOCK_STREAM, 0);
		ALWAYS_ASSERT(sock > 0);

		int v6only = 0;
		setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, (char *)&v6only, sizeof(v6only));

		connect(sock, (struct sockaddr*)&addr, sizeof(addr));
		close(sock);
	}
	connect_durations[node] = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start);
}

int main(int argc, char** argv) {
	bool validPort = false;
	try { std::stoul(argv[2]); validPort = true; } catch (std::exception& e) {}
	if ((argc != 3 && argc != 4) || !validPort) {
		printf("USAGE: %s BITCOIND_ADDRESS BITCOIND_PORT [ server ]\n", argv[0]);
		printf("Relay server is automatically selected by pinging available servers, unless one is specified\n");
		return -1;
	}

#ifdef WIN32
	WSADATA wsaData;
	if (WSAStartup(MAKEWORD(2,2), &wsaData))
		return -1;
#endif

	const char* relay = "public.%02d.relay.mattcorallo.com";
	char host[std::max(argc == 3 ? 0 : strlen(argv[3]), strlen(relay))];
	if (argc == 3) {
		while (true) {
			std::list<std::thread> threads;
			for (int i = 0; i < HOSTNAMES_TO_TEST; i++)
				threads.emplace_back(test_node, i);
			for (int i = 0; i < HOSTNAMES_TO_TEST; i++) {
				threads.front().join();
				threads.pop_front();
			}

			int min = -1; std::chrono::milliseconds min_duration(std::chrono::milliseconds::max());
			for (int i = 0; i < HOSTNAMES_TO_TEST; i++) {
				if (connect_durations[i] != std::chrono::milliseconds::max()) {
					std::string aka;
					sprintf(host, relay, i);
					printf("Server %d (%s) took %lld ms to respond %d times.\n", i, lookup_cname(host, aka) ? aka.c_str() : "", (long long int)connect_durations[i].count(), CONNECT_TESTS);
				}
				if (connect_durations[i] < min_duration) {
					min_duration = connect_durations[i];
					min = i;
				}
			}

			std::this_thread::sleep_for(std::chrono::seconds(10)); // Wait for server to open up our slot again
			if (min == -1) {
				printf("No servers responded\n");
				continue;
			}

			sprintf(host, relay, min);
			break;
		}
	} else
		memcpy(host, argv[3], strlen(argv[3]) + 1);
	printf("Using server %s\n", host);

	std::atomic_int relayToken(0);
	RelayNetworkClient* relayClient;
	P2PClient p2p(argv[1], std::stoul(argv[2]),
					[&](std::vector<unsigned char>& bytes, const std::chrono::system_clock::time_point&) { relayClient->receive_block(bytes, relayToken); },
					[&](std::shared_ptr<std::vector<unsigned char> >& bytes) {
						relayClient->receive_transaction(bytes, relayToken, !p2p.maybe_supports_mempool());
					},
					[&]() {
						relayClient->release_send_mutex(relayToken);
						relayToken = 0;
						if (!p2p.maybe_supports_mempool())
							p2p.regularly_request_mempool_and_dont_fetch_loose_txn = false;
					});
	relayClient = new RelayNetworkClient(host,
										[&](std::vector<unsigned char>& bytes) { p2p.receive_block(bytes); },
										[&](std::shared_ptr<std::vector<unsigned char> >& bytes) {
											p2p.receive_transaction(bytes);
											if (!p2p.maybe_supports_mempool())
												relayClient->receive_transaction(bytes, relayToken, false);
										},
										[&]() {
											relayToken = relayClient->get_send_mutex();
											relayClient->do_throttle_outbound(relayToken);
											p2p.request_mempool();
										});

	while (true) { sleep(1000); }
}
<|endoftext|>


<commit_before>#pragma once

class FindTests
    : public ::testing::Test
{
public:
    using types = IntervalTypes <int>;
protected:
    IntervalTypes <int>::tree_type tree;
};

TEST_F(FindTests, WillReturnEndIfTreeIsEmpty)
{
    EXPECT_EQ(tree.find({2, 7}), std::end(tree));
}

TEST_F(FindTests, WillNotFindRootIfItIsntTheSame)
{
    tree.insert({0, 1});
    EXPECT_EQ(tree.find({2, 7}), std::end(tree));
}

TEST_F(FindTests, WillFindRoot)
{
    tree.insert({0, 1});
    EXPECT_EQ(tree.find({0, 1}), std::begin(tree));
}

TEST_F(FindTests, WillFindInBiggerTree)
{
    tree.insert({16, 21});
    tree.insert({8, 9});
    tree.insert({25, 30});
    tree.insert({5, 8});
    tree.insert({15, 23});
    tree.insert({17, 19});
    tree.insert({26, 26});
    tree.insert({0, 3});
    tree.insert({6, 10});
    tree.insert({19, 20});
    auto iter = tree.find({15, 23});
    EXPECT_NE(iter, std::end(tree));
    EXPECT_EQ(iter->low(), 15);
    EXPECT_EQ(iter->high(), 23);
}

TEST_F(FindTests, WillFindAllInTreeWithDuplicates)
{
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    int findCount = 0;
    tree.find_all({5, 8}, [&findCount](decltype(tree)::iterator iter){
        ++findCount;
        EXPECT_EQ(*iter, (decltype(tree)::interval_type{5, 8}));
        return true;
    });
    EXPECT_EQ(findCount, tree.size());
}

TEST_F(FindTests, WillFindAllCanExitPreemptively)
{
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    int findCount = 0;
    tree.find_all({5, 8}, [&findCount](decltype(tree)::iterator iter){
        ++findCount;
        EXPECT_EQ(*iter, (decltype(tree)::interval_type{5, 8}));
        return findCount < 3;
    });
    EXPECT_EQ(findCount, 3);
}

<commit_msg>Fixed merge issue and added simple tests.<commit_after>#pragma once

class FindTests
    : public ::testing::Test
{
public:
    using types = IntervalTypes <int>;
protected:
    IntervalTypes <int>::tree_type tree;
    std::default_random_engine gen;
    std::uniform_int_distribution <int> distSmall{-500, 500};
    std::uniform_int_distribution <int> distLarge{-50000, 50000};
};

TEST_F(FindTests, WillReturnEndIfTreeIsEmpty)
{
    EXPECT_EQ(tree.find({2, 7}), std::end(tree));
}

TEST_F(FindTests, WillNotFindRootIfItIsntTheSame)
{
    tree.insert({0, 1});
    EXPECT_EQ(tree.find({2, 7}), std::end(tree));
}

TEST_F(FindTests, WillFindRoot)
{
    tree.insert({0, 1});
    EXPECT_EQ(tree.find({0, 1}), std::begin(tree));
}

TEST_F(FindTests, WillFindInBiggerTree)
{
    tree.insert({16, 21});
    tree.insert({8, 9});
    tree.insert({25, 30});
    tree.insert({5, 8});
    tree.insert({15, 23});
    tree.insert({17, 19});
    tree.insert({26, 26});
    tree.insert({0, 3});
    tree.insert({6, 10});
    tree.insert({19, 20});
    auto iter = tree.find({15, 23});
    EXPECT_NE(iter, std::end(tree));
    EXPECT_EQ(iter->low(), 15);
    EXPECT_EQ(iter->high(), 23);
}

TEST_F(FindTests, WillFindAllInTreeWithDuplicates)
{
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    int findCount = 0;
    tree.find_all({5, 8}, [&findCount](decltype(tree)::iterator iter){
        ++findCount;
        EXPECT_EQ(*iter, (decltype(tree)::interval_type{5, 8}));
        return true;
    });
    EXPECT_EQ(findCount, tree.size());
}

TEST_F(FindTests, WillFindAllCanExitPreemptively)
{
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    tree.insert({5, 8});
    int findCount = 0;
    tree.find_all({5, 8}, [&findCount](decltype(tree)::iterator iter){
        ++findCount;
        EXPECT_EQ(*iter, (decltype(tree)::interval_type{5, 8}));
        return findCount < 3;
    });
    EXPECT_EQ(findCount, 3);
}

TEST_F(FindTests, CanFindAllElementsBack)
{
    constexpr int amount = 10'000;

    std::vector <decltype(tree)::interval_type> intervals;
    intervals.reserve(amount);
    for (int i = 0; i != amount; ++i)
    {
        const auto interval = lib_interval_tree::make_safe_interval(distLarge(gen), distLarge(gen));
        intervals.emplace_back(interval);
        tree.insert(interval);
    }
    for (auto const& ival : intervals)
    {
        ASSERT_NE(tree.find(ival), std::end(tree));
    }
}

TEST_F(FindTests, CanFindAllElementsBackInStrictlyAscendingNonOverlappingIntervals)
{
    constexpr int amount = 10'000;

    std::vector <decltype(tree)::interval_type> intervals;
    intervals.reserve(amount);
    for (int i = 0; i != amount; ++i)
    {
        const auto interval = lib_interval_tree::make_safe_interval(i * 2,  i * 2 + 1);
        intervals.emplace_back(interval);
        tree.insert(interval);
    }
    for (auto const& ival : intervals)
    {
        ASSERT_NE(tree.find(ival), std::end(tree));
    }
}

TEST_F(FindTests, CanFindAllElementsBackInStrictlyAscendingOverlappingIntervals)
{
    constexpr int amount = 10'000;

    std::vector <decltype(tree)::interval_type> intervals;
    intervals.reserve(amount);
    for (int i = 0; i != amount; ++i)
    {
        const auto interval = lib_interval_tree::make_safe_interval(i - 1,  i + 1);
        intervals.emplace_back(interval);
        tree.insert(interval);
    }
    for (auto const& ival : intervals)
    {
        ASSERT_NE(tree.find(ival), std::end(tree));
    }
}

<|endoftext|>


<commit_before>#include "RenderTarget.hpp"

RenderTarget* RenderTarget::current = nullptr;

RenderTarget::RenderBuffer::RenderBuffer(int width, int height, Texture::InternalFormat format) : format(format){
	GL_ASSERT(glGenRenderbuffers(1, &handle));
	resize(width, height);
}

RenderTarget::RenderBuffer::~RenderBuffer() {
	GL_ASSERT(glDeleteRenderbuffers(1, &handle));
}

void RenderTarget::RenderBuffer::resize(int width, int height) {
	bind();
	GL_ASSERT(glRenderbufferStorage(GL_RENDERBUFFER, format, width, height));
}

void RenderTarget::RenderBuffer::bind() const {
	GL_ASSERT(glBindRenderbuffer(GL_RENDERBUFFER, handle));
}

GLuint RenderTarget::RenderBuffer::getHandle() const {
	return handle;
}

RenderTarget::RenderTarget(int width, int height) : handle(0), size(width, height), screenRelativeSize(false) {
	VBE_ASSERT(width != 0 && height != 0, "Width or height can't be zero");
}

RenderTarget::RenderTarget(float mult) : handle(0), size(int(Environment::getScreen()->getWidth()*mult), int(Environment::getScreen()->getHeight()*mult)), screenRelativeSize(true), screenSizeMultiplier(mult) {
}

RenderTarget::RenderTarget() : handle(0), size(int(Environment::getScreen()->getWidth()), int(Environment::getScreen()->getHeight())), screenRelativeSize(true), screenSizeMultiplier(1) {
}

RenderTarget::~RenderTarget() {
	if(handle != 0)
		destroy();
}

void RenderTarget::bind(RenderTarget* target) {
	if(current == target) return;
	if(target == nullptr) {
		GL_ASSERT(glViewport(0, 0, Environment::getScreen()->getWidth(), Environment::getScreen()->getHeight()));
		GL_ASSERT(glBindFramebuffer(GL_FRAMEBUFFER, 0));
	}
	else {
		VBE_ASSERT(target->handle != 0, "Cannot bind unbuilt render target");
		target->checkSize();
		GL_ASSERT(glViewport(0, 0, target->size.x, target->size.y));
		GL_ASSERT(glBindFramebuffer(GL_FRAMEBUFFER, target->handle));
	}
	current = target;
}

RenderTarget* RenderTarget::getCurrent() {
	return current;
}

void RenderTarget::addRenderBuffer(RenderTarget::Attachment attachment, Texture::InternalFormat format) {
	VBE_ASSERT(handle == 0, "Cannot add RenderBuffer to already built RenderTarget. Call RenderTarget::destroy() first or add before building");
	VBE_ASSERT(entries.find(attachment) == entries.end(), "There's already an entry with the requested Attachment");
	entries.insert(std::pair<Attachment, RenderTargetEntry> (attachment, RenderTargetEntry(RenderTargetEntry::RenderBufferEntry, attachment, format)));
}

void RenderTarget::addTexture(RenderTarget::Attachment attachment, Texture::InternalFormat format) {
	VBE_ASSERT(handle == 0, "Cannot add texture to already built RenderTarget. Call RenderTarget::destroy() first or add before building");
	VBE_ASSERT(entries.find(attachment) == entries.end(), "There's already an entry with the requested Attachment");
	entries.insert(std::pair<Attachment, RenderTargetEntry> (attachment, RenderTargetEntry(RenderTargetEntry::TextureEntry, attachment, format)));
}

void RenderTarget::addCustomTexture(RenderTarget::Attachment attachment, Texture2D* tex) {
	VBE_ASSERT(handle == 0, "Cannot add texture to already built RenderTarget. Call RenderTarget::destroy() first or add before building");
	VBE_ASSERT(entries.find(attachment) == entries.end(), "There's already an entry with the requested Attachment");
	entries.insert(std::pair<Attachment, RenderTargetEntry> (attachment, RenderTargetEntry(attachment, tex)));
}

void RenderTarget::setCustomTexture(RenderTarget::Attachment attachment, Texture2D* tex) {
	VBE_ASSERT(handle == 0, "Cannot set a texture on an already built RenderTarget. Call RenderTarget::destroy() first or set before building");
	VBE_ASSERT(entries.find(attachment) != entries.end(), "There's no entry with the requested Attachment");
	RenderTargetEntry& e = entries.at(attachment);
	VBE_ASSERT(e.type == RenderTargetEntry::TextureEntry, "Cannot set the texture of a RenderBuffer entry.");
	VBE_ASSERT(e.user, "Cannot set the texture of a non user-managed Texture entry.");
	e.texture = tex;
}

void RenderTarget::build() {
	VBE_ASSERT(handle == 0, "Cannot rebuild already built RenderTarget. Call RenderTarget::destroy() first");
	VBE_ASSERT(entries.size() != 0, "This RenderTarget has no textures or render buffers.");

	size = getDesiredSize();

	GL_ASSERT(glGenFramebuffers(1, &handle));
	RenderTarget* current = getCurrent();
	bind(this);

	std::vector<Attachment> drawAttachments;

	for(std::map<Attachment, RenderTargetEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
		RenderTargetEntry& e = it->second;

		if(e.type == RenderTargetEntry::RenderBufferEntry) {
			RenderBuffer* buff = new RenderBuffer(size.x, size.y, e.format);
			buff->bind();
			GL_ASSERT(glFramebufferRenderbuffer(GL_FRAMEBUFFER, e.attachment, GL_RENDERBUFFER, buff->getHandle()));
			e.renderBuffer = buff;
		}
		else {
			Texture2D* tex = Texture2D::createEmpty(size.x, size.y, e.format);
			tex->bind();
			GL_ASSERT(glFramebufferTexture(GL_FRAMEBUFFER, e.attachment, tex->getHandle(), 0));
			e.texture = tex;
		}

		if(e.attachment >= COLOR0 && e.attachment <= COLOR15)
			drawAttachments.push_back(e.attachment);
	}

	if(drawAttachments.size() == 0) {
		GLenum none = GL_NONE;
		GL_ASSERT(glDrawBuffers(1, &none));
	}
	else
		GL_ASSERT(glDrawBuffers(drawAttachments.size(), (GLenum*)&drawAttachments[0]));

	VBE_ASSERT(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE, "Can't create framebuffer");

	bind(current);
}

void RenderTarget::checkSize() {
	VBE_ASSERT(handle != 0, "RenderTarget should be built");

	vec2i desiredSize = getDesiredSize();
	if(getSize() == desiredSize)
		return;

	VBE_LOG("Resizing RenderTarget");

	for(std::map<Attachment, RenderTargetEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
		RenderTargetEntry& e = it->second;
		if(e.user) continue;
		if(e.type == RenderTargetEntry::RenderBufferEntry)
			e.renderBuffer->resize(desiredSize.x, desiredSize.y);
		else
			e.texture->resize(desiredSize.x, desiredSize.y);
	}

	size = getDesiredSize();
}

void RenderTarget::destroy() {
	VBE_ASSERT(handle != 0, "Can't destroy an unbuilt RenderTarget");

	for(std::map<Attachment, RenderTargetEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
		RenderTargetEntry& e = it->second;
		if(e.user) continue;

		if(e.type == RenderTargetEntry::RenderBufferEntry) {
			delete e.renderBuffer;
			e.renderBuffer = nullptr;
		}
		else {
			delete e.texture;
			e.texture = nullptr;
		}
	}
	GL_ASSERT(glDeleteFramebuffers(1, &handle));

	handle = 0;
}

const Texture2D* RenderTarget::getTextureForAttachment(RenderTarget::Attachment target) const {
	VBE_ASSERT(handle != 0, "Can't get texture for attachment without building it first");
	VBE_ASSERT(entries.find(target) != entries.end(), "Trying to retrieve unexisting texture from RenderTarget");
	const RenderTargetEntry& e = entries.at(target);
	VBE_ASSERT(e.type == RenderTargetEntry::TextureEntry, "You can't get a texture for a RenderBuffer attachment");
	return e.texture;
}

Texture2D* RenderTarget::getTextureForAttachment(RenderTarget::Attachment target) {
	VBE_ASSERT(handle != 0, "Can't get texture for attachment without building it first");
	VBE_ASSERT(entries.find(target) != entries.end(), "Trying to retrieve unexisting texture from RenderTarget");
	RenderTargetEntry& e = entries.at(target);
	VBE_ASSERT(e.type == RenderTargetEntry::TextureEntry, "You can't get a texture for a RenderBuffer attachment");
	return e.texture;
}
<commit_msg>Fixed rebuilding rendertarget bug<commit_after>#include "RenderTarget.hpp"

RenderTarget* RenderTarget::current = nullptr;

RenderTarget::RenderBuffer::RenderBuffer(int width, int height, Texture::InternalFormat format) : format(format){
	GL_ASSERT(glGenRenderbuffers(1, &handle));
	resize(width, height);
}

RenderTarget::RenderBuffer::~RenderBuffer() {
	GL_ASSERT(glDeleteRenderbuffers(1, &handle));
}

void RenderTarget::RenderBuffer::resize(int width, int height) {
	bind();
	GL_ASSERT(glRenderbufferStorage(GL_RENDERBUFFER, format, width, height));
}

void RenderTarget::RenderBuffer::bind() const {
	GL_ASSERT(glBindRenderbuffer(GL_RENDERBUFFER, handle));
}

GLuint RenderTarget::RenderBuffer::getHandle() const {
	return handle;
}

RenderTarget::RenderTarget(int width, int height) : handle(0), size(width, height), screenRelativeSize(false) {
	VBE_ASSERT(width != 0 && height != 0, "Width or height can't be zero");
}

RenderTarget::RenderTarget(float mult) : handle(0), size(int(Environment::getScreen()->getWidth()*mult), int(Environment::getScreen()->getHeight()*mult)), screenRelativeSize(true), screenSizeMultiplier(mult) {
}

RenderTarget::RenderTarget() : handle(0), size(int(Environment::getScreen()->getWidth()), int(Environment::getScreen()->getHeight())), screenRelativeSize(true), screenSizeMultiplier(1) {
}

RenderTarget::~RenderTarget() {
	if(handle != 0)
		destroy();
}

void RenderTarget::bind(RenderTarget* target) {
	if(current == target) return;
	if(target == nullptr) {
		GL_ASSERT(glViewport(0, 0, Environment::getScreen()->getWidth(), Environment::getScreen()->getHeight()));
		GL_ASSERT(glBindFramebuffer(GL_FRAMEBUFFER, 0));
	}
	else {
		VBE_ASSERT(target->handle != 0, "Cannot bind unbuilt render target");
		target->checkSize();
		GL_ASSERT(glViewport(0, 0, target->size.x, target->size.y));
		GL_ASSERT(glBindFramebuffer(GL_FRAMEBUFFER, target->handle));
	}
	current = target;
}

RenderTarget* RenderTarget::getCurrent() {
	return current;
}

void RenderTarget::addRenderBuffer(RenderTarget::Attachment attachment, Texture::InternalFormat format) {
	VBE_ASSERT(handle == 0, "Cannot add RenderBuffer to already built RenderTarget. Call RenderTarget::destroy() first or add before building");
	VBE_ASSERT(entries.find(attachment) == entries.end(), "There's already an entry with the requested Attachment");
	entries.insert(std::pair<Attachment, RenderTargetEntry> (attachment, RenderTargetEntry(RenderTargetEntry::RenderBufferEntry, attachment, format)));
}

void RenderTarget::addTexture(RenderTarget::Attachment attachment, Texture::InternalFormat format) {
	VBE_ASSERT(handle == 0, "Cannot add texture to already built RenderTarget. Call RenderTarget::destroy() first or add before building");
	VBE_ASSERT(entries.find(attachment) == entries.end(), "There's already an entry with the requested Attachment");
	entries.insert(std::pair<Attachment, RenderTargetEntry> (attachment, RenderTargetEntry(RenderTargetEntry::TextureEntry, attachment, format)));
}

void RenderTarget::addCustomTexture(RenderTarget::Attachment attachment, Texture2D* tex) {
	VBE_ASSERT(handle == 0, "Cannot add texture to already built RenderTarget. Call RenderTarget::destroy() first or add before building");
	VBE_ASSERT(entries.find(attachment) == entries.end(), "There's already an entry with the requested Attachment");
	entries.insert(std::pair<Attachment, RenderTargetEntry> (attachment, RenderTargetEntry(attachment, tex)));
}

void RenderTarget::setCustomTexture(RenderTarget::Attachment attachment, Texture2D* tex) {
	VBE_ASSERT(handle == 0, "Cannot set a texture on an already built RenderTarget. Call RenderTarget::destroy() first or set before building");
	VBE_ASSERT(entries.find(attachment) != entries.end(), "There's no entry with the requested Attachment");
	RenderTargetEntry& e = entries.at(attachment);
	VBE_ASSERT(e.type == RenderTargetEntry::TextureEntry, "Cannot set the texture of a RenderBuffer entry.");
	VBE_ASSERT(e.user, "Cannot set the texture of a non user-managed Texture entry.");
	e.texture = tex;
}

void RenderTarget::build() {
	VBE_ASSERT(handle == 0, "Cannot rebuild already built RenderTarget. Call RenderTarget::destroy() first");
	VBE_ASSERT(entries.size() != 0, "This RenderTarget has no textures or render buffers.");

	size = getDesiredSize();

	GL_ASSERT(glGenFramebuffers(1, &handle));
	RenderTarget* current = getCurrent();
	bind(this);

	std::vector<Attachment> drawAttachments;

	for(std::map<Attachment, RenderTargetEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
		RenderTargetEntry& e = it->second;

		if(e.type == RenderTargetEntry::RenderBufferEntry) {
			RenderBuffer* buff = new RenderBuffer(size.x, size.y, e.format);
			buff->bind();
			GL_ASSERT(glFramebufferRenderbuffer(GL_FRAMEBUFFER, e.attachment, GL_RENDERBUFFER, buff->getHandle()));
			e.renderBuffer = buff;
		}
		else {
			Texture2D* tex = (e.user ? e.texture : Texture2D::createEmpty(size.x, size.y, e.format));
			tex->bind();
			GL_ASSERT(glFramebufferTexture(GL_FRAMEBUFFER, e.attachment, tex->getHandle(), 0));
			e.texture = tex;
		}

		if(e.attachment >= COLOR0 && e.attachment <= COLOR15)
			drawAttachments.push_back(e.attachment);
	}

	if(drawAttachments.size() == 0) {
		GLenum none = GL_NONE;
		GL_ASSERT(glDrawBuffers(1, &none));
	}
	else
		GL_ASSERT(glDrawBuffers(drawAttachments.size(), (GLenum*)&drawAttachments[0]));

	VBE_ASSERT(glCheckFramebufferStatus(GL_FRAMEBUFFER) == GL_FRAMEBUFFER_COMPLETE, "Can't create framebuffer");

	bind(current);
}

void RenderTarget::checkSize() {
	VBE_ASSERT(handle != 0, "RenderTarget should be built");

	vec2i desiredSize = getDesiredSize();
	if(getSize() == desiredSize)
		return;

	VBE_LOG("Resizing RenderTarget");

	for(std::map<Attachment, RenderTargetEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
		RenderTargetEntry& e = it->second;
		if(e.user) continue;
		if(e.type == RenderTargetEntry::RenderBufferEntry)
			e.renderBuffer->resize(desiredSize.x, desiredSize.y);
		else
			e.texture->resize(desiredSize.x, desiredSize.y);
	}

	size = getDesiredSize();
}

void RenderTarget::destroy() {
	VBE_ASSERT(handle != 0, "Can't destroy an unbuilt RenderTarget");

	for(std::map<Attachment, RenderTargetEntry>::iterator it = entries.begin(); it != entries.end(); ++it) {
		RenderTargetEntry& e = it->second;
		if(e.user) continue;

		if(e.type == RenderTargetEntry::RenderBufferEntry) {
			delete e.renderBuffer;
			e.renderBuffer = nullptr;
		}
		else {
			delete e.texture;
			e.texture = nullptr;
		}
	}
	GL_ASSERT(glDeleteFramebuffers(1, &handle));

	handle = 0;
}

const Texture2D* RenderTarget::getTextureForAttachment(RenderTarget::Attachment target) const {
	VBE_ASSERT(handle != 0, "Can't get texture for attachment without building it first");
	VBE_ASSERT(entries.find(target) != entries.end(), "Trying to retrieve unexisting texture from RenderTarget");
	const RenderTargetEntry& e = entries.at(target);
	VBE_ASSERT(e.type == RenderTargetEntry::TextureEntry, "You can't get a texture for a RenderBuffer attachment");
	return e.texture;
}

Texture2D* RenderTarget::getTextureForAttachment(RenderTarget::Attachment target) {
	VBE_ASSERT(handle != 0, "Can't get texture for attachment without building it first");
	VBE_ASSERT(entries.find(target) != entries.end(), "Trying to retrieve unexisting texture from RenderTarget");
	RenderTargetEntry& e = entries.at(target);
	VBE_ASSERT(e.type == RenderTargetEntry::TextureEntry, "You can't get a texture for a RenderBuffer attachment");
	return e.texture;
}
<|endoftext|>


<commit_before>#include "PackUIWindowTask.h"
#include "PackUI.h"
#include "pack_ui_cfg.h"

#include <ee/FileHelper.h>
#include <ee/std_functor.h>
#include <ee/StringHelper.h>

#include <algorithm>

namespace erespacker
{

PackUIWindowTask::PackUIWindowTask(const std::string& filepath, const Json::Value& value)
	: PackUITask(filepath)
	, m_wrapper_id(-1)
{
	m_name = value["name"].asString();

	m_width = value["view"]["width"].asInt();
	m_height = value["view"]["height"].asInt();

	m_wrapper_filepath = value["wrapper filepath"].asString();
	m_wrapper_filepath = ee::FileHelper::GetAbsolutePathFromFile(filepath, m_wrapper_filepath);
	m_wrapper_filepath = ee::FileHelper::FormatFilepathAbsolute(m_wrapper_filepath);
	PackUI::Instance()->Instance()->AddListener(m_wrapper_filepath, this);

	LoadItems(value, ee::FileHelper::GetFileDir(filepath));
}

PackUIWindowTask::~PackUIWindowTask()
{
	for_each(m_items.begin(), m_items.end(), ee::DeletePointerFunctor<Item>());
}

void PackUIWindowTask::OnKnownPackID(const std::string& filepath, int id)
{
	if (filepath == m_wrapper_filepath) {
		m_wrapper_id = id;
		return;
	}

	for (int i = 0, n = m_items.size(); i < n; ++i) {
		Item* item = m_items[i];
		if (item->filepath == filepath) {
			item->id = id;
		}
	}
}

void PackUIWindowTask::Output(const std::string& dir, Json::Value& value) const
{
	Json::Value val;

	val["type"] = UI_WINDOW;

	val["name"] = ee::StringHelper::ToUtf8(m_name);

	val["width"] = m_width;
	val["height"] = m_height;

	val["id"] = m_wrapper_id;

	for (int i = 0, n = m_items.size(); i < n; ++i) {
		Item* item = m_items[i];
		Json::Value item_val;
		item_val["id"] = item->id;
		item_val["anchor"] = item->anchor;
		val["sprite"][i] = item_val;
	}

	value[value.size()] = val;
}

void PackUIWindowTask::LoadItems(const Json::Value& value, const std::string& dir)
{
	int idx = 0;
	Json::Value spr_val = value["sprite"][idx++];
	while (!spr_val.isNull()) {
		Item* item = new Item;

		item->name = spr_val["name"].asString();
		item->filepath = spr_val["filepath"].asString();
		item->id = -1;
		item->anchor = -1;

		item->filepath = ee::FileHelper::GetAbsolutePath(dir, item->filepath);
		item->filepath = ee::FileHelper::FormatFilepathAbsolute(item->filepath);
		PackUI::Instance()->Instance()->AddListener(item->filepath, this);

		m_items.push_back(item);

		spr_val = value["sprite"][idx++];
	}

	for (int i = 0; i < 9; ++i)
	{
		Json::Value anchor_val = value["anchor"][i];
		if (anchor_val.isNull()) {
			continue;
		}

		int idx = 0;
		Json::Value spr_val = anchor_val[idx++];
		while (!spr_val.isNull()) {
			std::string name = spr_val["name"].asString();
			Item* item = NULL;
			for (int j = 0, m = m_items.size(); j < m; ++j) {
				if (name == m_items[j]->name) {
					item = m_items[j];
					break;
				}
			}

			assert(item);
			item->anchor = i;

			spr_val = anchor_val[idx++];
		}
	}

	idx = 0;
	Json::Value ref_val = value["ref_spr"][idx++];
	while (!spr_val.isNull()) {
		std::string filepath = ref_val["filepath"].asString();
		filepath = ee::FileHelper::GetAbsolutePath(dir, filepath);

		Json::Value ext_val;
		Json::Reader reader;
		std::locale::global(std::locale(""));
		std::ifstream fin(filepath.c_str());
		std::locale::global(std::locale("C"));
		reader.parse(fin, ext_val);
		fin.close();

		LoadItems(ext_val, ee::FileHelper::GetFileDir(filepath));

		ref_val = value["ref_spr"][idx++];
	}
}

}<commit_msg>[ADDED] exception for id = -1<commit_after>#include "PackUIWindowTask.h"
#include "PackUI.h"
#include "pack_ui_cfg.h"

#include <ee/FileHelper.h>
#include <ee/std_functor.h>
#include <ee/StringHelper.h>
#include <ee/Exception.h>

#include <algorithm>

namespace erespacker
{

PackUIWindowTask::PackUIWindowTask(const std::string& filepath, const Json::Value& value)
	: PackUITask(filepath)
	, m_wrapper_id(-1)
{
	m_name = value["name"].asString();

	m_width = value["view"]["width"].asInt();
	m_height = value["view"]["height"].asInt();

	m_wrapper_filepath = value["wrapper filepath"].asString();
	m_wrapper_filepath = ee::FileHelper::GetAbsolutePathFromFile(filepath, m_wrapper_filepath);
	m_wrapper_filepath = ee::FileHelper::FormatFilepathAbsolute(m_wrapper_filepath);

	PackUI::Instance()->Instance()->AddListener(m_wrapper_filepath, this);

	LoadItems(value, ee::FileHelper::GetFileDir(filepath));
}

PackUIWindowTask::~PackUIWindowTask()
{
	for_each(m_items.begin(), m_items.end(), ee::DeletePointerFunctor<Item>());
}

void PackUIWindowTask::OnKnownPackID(const std::string& filepath, int id)
{
	if (filepath == m_wrapper_filepath) {
		m_wrapper_id = id;
		return;
	}

	for (int i = 0, n = m_items.size(); i < n; ++i) {
		Item* item = m_items[i];
		if (item->filepath == filepath) {
			item->id = id;
		}
	}
}

void PackUIWindowTask::Output(const std::string& dir, Json::Value& value) const
{
	if (m_wrapper_id == -1) {
		throw ee::Exception("PackUIWindowTask::Output out id -1, wrapper_file %s", m_wrapper_filepath.c_str());
	}

	Json::Value val;

	val["type"] = UI_WINDOW;

	val["name"] = ee::StringHelper::ToUtf8(m_name);

	val["width"] = m_width;
	val["height"] = m_height;

	val["id"] = m_wrapper_id;

	for (int i = 0, n = m_items.size(); i < n; ++i) {
		Item* item = m_items[i];
		Json::Value item_val;
		item_val["id"] = item->id;
		item_val["anchor"] = item->anchor;
		val["sprite"][i] = item_val;
	}

	value[value.size()] = val;
}

void PackUIWindowTask::LoadItems(const Json::Value& value, const std::string& dir)
{
	int idx = 0;
	Json::Value spr_val = value["sprite"][idx++];
	while (!spr_val.isNull()) {
		Item* item = new Item;

		item->name = spr_val["name"].asString();
		item->filepath = spr_val["filepath"].asString();
		item->id = -1;
		item->anchor = -1;

		item->filepath = ee::FileHelper::GetAbsolutePath(dir, item->filepath);
		item->filepath = ee::FileHelper::FormatFilepathAbsolute(item->filepath);
		PackUI::Instance()->Instance()->AddListener(item->filepath, this);

		m_items.push_back(item);

		spr_val = value["sprite"][idx++];
	}

	for (int i = 0; i < 9; ++i)
	{
		Json::Value anchor_val = value["anchor"][i];
		if (anchor_val.isNull()) {
			continue;
		}

		int idx = 0;
		Json::Value spr_val = anchor_val[idx++];
		while (!spr_val.isNull()) {
			std::string name = spr_val["name"].asString();
			Item* item = NULL;
			for (int j = 0, m = m_items.size(); j < m; ++j) {
				if (name == m_items[j]->name) {
					item = m_items[j];
					break;
				}
			}

			assert(item);
			item->anchor = i;

			spr_val = anchor_val[idx++];
		}
	}

	idx = 0;
	Json::Value ref_val = value["ref_spr"][idx++];
	while (!spr_val.isNull()) {
		std::string filepath = ref_val["filepath"].asString();
		filepath = ee::FileHelper::GetAbsolutePath(dir, filepath);

		Json::Value ext_val;
		Json::Reader reader;
		std::locale::global(std::locale(""));
		std::ifstream fin(filepath.c_str());
		std::locale::global(std::locale("C"));
		reader.parse(fin, ext_val);
		fin.close();

		LoadItems(ext_val, ee::FileHelper::GetFileDir(filepath));

		ref_val = value["ref_spr"][idx++];
	}
}

}<|endoftext|>


<commit_before>#include "predicator.h"

/**
  predicator_planning
  Can generate the set of predicates that would be true for a given location.
  Also used to generate predicates based on current state.
 **/
namespace predicator_planning {

  /*
   * createStatement()
   * Simple helper function to create predicates with
   */
  PredicateStatement createStatement(std::string predicate, double value, std::string param1, std::string param2, std::string param3 = "") {
    PredicateStatement ps;
    ps.predicate = predicate;
    ps.params[0] = param1;
    ps.params[1] = param2;
    ps.value = value;

    int num_params = 2;

    if (param3.size() > 0) {
      ++num_params;
      ps.params[2] = param3;
    }
    ps.num_params = num_params;

    return ps;
  }

  /*
   * joint_state_callback()
   * Update the robot state variable values
   */
  void joint_state_callback(const sensor_msgs::JointState::ConstPtr &msg, RobotState *state) {
    state->setVariableValues(*msg);
  }

  PredicateContext::PredicateContext(bool publish) {
    ros::NodeHandle nh_tilde("~");
    ros::NodeHandle nh;

    XmlRpc::XmlRpcValue frames_list;
    XmlRpc::XmlRpcValue descriptions;
    XmlRpc::XmlRpcValue topics;
    XmlRpc::XmlRpcValue floating; // set of floating root joints that need to be updated

    nh_tilde.param("verbosity", verbosity, 0);
    nh_tilde.param("padding", padding, 0.01);
    nh_tilde.param("world_frame", world_frame, std::string("/world"));

    // should we publish predicate messages?
    // or what?
    if(publish == true) {
      pub = nh.advertise<predicator_msgs::PredicateList>("/predicator/input", 1000);
      vpub = nh.advertise<predicator_msgs::ValidPredicates>("/predicator/valid_input", 1000);
    }

    if(nh_tilde.hasParam("description_list")) {
      nh_tilde.param("description_list", descriptions, descriptions);
    } else {
      ROS_ERROR("No list of robot description parameters!");
      exit(-1);
    }

    if(nh_tilde.hasParam("frames")) {
      nh_tilde.param("frames", frames_list, frames_list);
    } else {
      ROS_ERROR("No list of frames for geometryic predicates!");
      exit(-1);
    }

    if(nh_tilde.hasParam("joint_state_topic_list")) {
      nh_tilde.param("joint_state_topic_list", topics, topics);
    } else {
      ROS_ERROR("No list of joint state topics!");
      exit(-1);
    }

    bool load_floating = false;
    if(nh_tilde.hasParam("floating_root_list")) {
      nh_tilde.param("floating_root_list", floating, floating);
      load_floating = true;
    } else {
      ROS_INFO("No list of robots with floating root joints given.");
    }

    if(descriptions.size() != topics.size()) {
      ROS_WARN("An unequal number of joint state and robot topics was provided!");
    }

    // define valid predicates topic
    pval.predicates.push_back("touching");
    pval.value_predicates.push_back("mesh_distance");
    pval.pheader.source = ros::this_node::getName();

    // read in topics and descriptions
    for(unsigned int i = 0; i < descriptions.size(); ++i) {
      std::string desc;
      std::string topic;

      if(descriptions[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        desc = static_cast<std::string>(descriptions[i]);
        if(verbosity > 0) {
          std::cout << "Robot Description parameter name: " << desc << std::endl;
        }
      } else {
        ROS_WARN("Description %u was not of type \"string\"!", i);
        continue;
      }

      // create a robot model with state desc
      robot_model_loader::RobotModelLoader robot_model_loader(desc);
      robot_model::RobotModelPtr model = robot_model_loader.getModel();
      PlanningScene *scene = new PlanningScene(model);
      scene->getCollisionRobotNonConst()->setPadding(padding);
      scene->propogateRobotPadding();

      // get all link names as possible assignments
      for(typename std::vector<std::string>::const_iterator it = model->getLinkModelNames().begin();
          it != model->getLinkModelNames().end();
          ++it)
      {
        pval.assignments.push_back(*it);
      }

      robots.push_back(model);
      scenes.push_back(scene);

      RobotState *state = new RobotState(model);
      states.push_back(state);

      if(i < topics.size() && topics[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        topic = static_cast<std::string>(topics[i]);
        if(verbosity > 0) {
          std::cout << "JointState topic name: " << topic << std::endl;
        }

        // create the subscriber
        subs.push_back(nh.subscribe<sensor_msgs::JointState>
                       (topic, 1000,
                        boost::bind(joint_state_callback, _1, state)));
      } else if (verbosity > 0) {
        ROS_WARN("no topic corresponding to description %s!", desc.c_str());
      }
    }

    // read in frames of interest
    for(unsigned int i = 0; i < frames_list.size(); ++i) {
      std::string frame;

      if(frames_list[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        frame = static_cast<std::string>(frames_list[i]);
        if(verbosity > 0) {
          std::cout << "Including frame: " << frame << std::endl;
        }
        frames.push_back(frame);
      } else {
        ROS_WARN("Frame list entry %u was not of type \"string\"!", i);
        continue;
      }
    }

    ROS_INFO("about to parse floating");
    if(load_floating) {
      // read in root TF frames
      for(unsigned int i = 0; i < floating.size(); ++i) {
        std::string id = floating[i]["id"];
        std::string frame = floating[i]["frame"];

        floating_frames[id] = frame;
      }
    }
    ROS_INFO("parsed floating");

    // print out information on all the different joints
    if(verbosity > 0) {
      unsigned int i = 0;
      for(typename std::vector<PlanningScene *>::iterator it1 = scenes.begin();
          it1 != scenes.end();
          ++it1, ++i)
      {
        collision_detection::CollisionRobotConstPtr robot1 = (*it1)->getCollisionRobot();
        // -----------------------------------------------------------
        std::cout << std::endl;
        std::cout << "PRINTING STATE INFO:";
        std::cout << robot1->getRobotModel()->getName() << std::endl;
        std::cout << robots[i]->getRootJointName() << std::endl;
        states[i]->update(true);
        states[i]->printStateInfo(std::cout);
        // -----------------------------------------------------------
      }
    }
  }

  /**
   * cleanup()
   * Delete memory allocated for robot states and subscribers
   */
  void PredicateContext::cleanup() {
    for (typename std::vector<RobotState *>::iterator it = states.begin();
         it != states.end();
         ++it)
    {
      delete *it;
    }

    for (typename std::vector<PlanningScene *>::iterator it = scenes.begin();
         it != scenes.end();
         ++it)
    {
      delete *it;
    }
  }

  /**
   * updatRobotStates()
   * make sure base frames are up to date
   * some objects, such as free-floating robots (aka the ring) need to be updated by TF
   * not sure why this doesn't work naturally
   */
  void PredicateContext::updateRobotStates() {
    unsigned int i = 0;

    for(typename std::vector<PlanningScene *>::iterator it1 = scenes.begin();
        it1 != scenes.end();
        ++it1, ++i)
    {

      collision_detection::CollisionRobotConstPtr robot1 = (*it1)->getCollisionRobot();
      std::string name = robot1->getRobotModel()->getName();

      if(floating_frames.find(name) != floating_frames.end()) {
        std::string base_frame = floating_frames[name];

        tf::StampedTransform transform;
        Eigen::Affine3d t;

        try{
          listener.lookupTransform(world_frame, base_frame,
                                   ros::Time(0), transform);
          tf::transformTFToEigen(transform,t);
          states[i]->setJointPositions(robot1->getRobotModel()->getRootJointName(), t);
        }
        catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
        }

        if(verbosity > 1) {
          std::cout << "----------------------------" << std::endl;
          std::cout << "PRINTING STATE INFO:";
          std::cout << robot1->getRobotModel()->getName() << std::endl;
          std::cout << robots[i]->getRootJointName() << std::endl;
          states[i]->update(true);
          states[i]->printStateInfo(std::cout);
        }

      } else {
        continue;
      }
    }
  }

  /**
   * addCollisionPredicates()
   * main collision checking loop
   * checks for all pairs of objects, determines collisions and distances
   * publishes the relationships between all of these objects
   */
  void PredicateContext::addCollisionPredicates(PredicateList &output, std::vector<double> &heuristics, const std::vector<RobotState *> &states) {

    unsigned i = 0;
    for(typename std::vector<PlanningScene *>::iterator it1 = scenes.begin();
        it1 != scenes.end();
        ++it1, ++i)
    {

      collision_detection::CollisionRobotConstPtr robot1 = (*it1)->getCollisionRobot();

      typename std::vector<PlanningScene *>::iterator it2 = it1;
      unsigned int j = i+1;
      for(++it2; it2 != scenes.end(); ++it2, ++j) {

        //if (i == j) continue;

        collision_detection::CollisionRobotConstPtr robot2 = (*it2)->getCollisionRobot();

        collision_detection::CollisionRequest req;
        collision_detection::CollisionResult res;
        req.contacts = true;
        req.max_contacts = 1000;

        // force an update
        // source: https://groups.google.com/forum/#!topic/moveit-users/O9CEef6sxbE
        states[i]->update(true);
        states[j]->update(true);
        robot1->checkOtherCollision(req, res, *states[i], *robot2, *states[j]);
        double dist = robot1->distanceOther(*states[i], *robot2, *states[j]);

        // write distance predicate
        predicator_msgs::PredicateStatement ps_dist;
        ps_dist.predicate = "mesh_distance";
        ps_dist.value = dist;
        ps_dist.num_params = 2;
        ps_dist.params[0] = robot1->getRobotModel()->getName();
        ps_dist.params[1] = robot2->getRobotModel()->getName();
        output.statements.push_back(ps_dist);

        // the reverse is also true, so write it as well
        predicator_msgs::PredicateStatement ps_dist2;
        ps_dist2.predicate = "mesh_distance";
        ps_dist2.value = dist;
        ps_dist2.num_params = 2;
        ps_dist2.params[0] = robot1->getRobotModel()->getName();
        ps_dist2.params[1] = robot2->getRobotModel()->getName();
        output.statements.push_back(ps_dist2);

        // iterate over all collisions
        for(collision_detection::CollisionResult::ContactMap::const_iterator cit = res.contacts.begin();
            cit != res.contacts.end();
            ++cit)
        {
          // write the correct predicate
          predicator_msgs::PredicateStatement ps;
          ps.predicate = "touching";
          ps.value = 1.0;
          ps.num_params = 2;
          ps.params[0] = cit->first.first;
          ps.params[1] = cit->first.second;
          output.statements.push_back(ps);

          // the reverse is also true, so update it
          predicator_msgs::PredicateStatement ps2;
          ps2.predicate = "touching";
          ps2.value = 1.0;
          ps2.num_params = 2;
          ps2.params[0] = cit->first.second;
          ps2.params[1] = cit->first.first;
          output.statements.push_back(ps2);
        }

        if (verbosity > 1) {
          std::cout << "(" << robot1->getRobotModel()->getName()
            << ", " << robot2->getRobotModel()->getName()
            << ") : Distance to collision: " << dist << std::endl;
        }
      }
    }

  }

  /**
   * tick()
   * Run one iteration of the predicator computations
   */
  void PredicateContext::tick() {
    predicator_msgs::PredicateList output;
    output.pheader.source = ros::this_node::getName();

    std::vector<double> heuristics;

    updateRobotStates();
    addCollisionPredicates(output, heuristics, states);
    addGeometryPredicates(output, heuristics, states);
    addReachabilityPredicates(output, heuristics, states);

    pub.publish(output);
    vpub.publish(pval);
  }

  /**
   * addReachabilityPredicates()
   * compute whether or not we can reach certain points or waypoints
   */
  void PredicateContext::addReachabilityPredicates(PredicateList &list, std::vector<double> &heuristics, const std::vector<RobotState *> &states) {

  }

  /**
   * addGeometryPredicates()
   * compute the set of geometry predicates
   *
   * Links for the different world objects from the peg demo:
      world wam/base_link wam/shoulder_yaw_link wam/shoulder_pitch_link wam/upper_arm_link wam/forearm_link wam/wrist_yaw_link wam/wrist_pitch_link wam/wrist_palm_link wam/hand/bhand_palm_link wam/hand/bhand_grasp_link wam/hand/bhand_palm_surface_link wam/hand/finger_1/prox_link wam/hand/finger_1/med_link wam/hand/finger_1/dist_link wam/hand/finger_2/prox_link wam/hand/finger_2/med_link wam/hand/finger_2/dist_link wam/hand/finger_3/med_link wam/hand/finger_3/dist_link wam/wrist_palm_stump_link
      world wam2/base_link wam2/shoulder_yaw_link wam2/shoulder_pitch_link wam2/upper_arm_link wam2/forearm_link wam2/wrist_yaw_link wam2/wrist_pitch_link wam2/wrist_palm_link wam2/hand/bhand_palm_link wam2/hand/bhand_grasp_link wam2/hand/bhand_palm_surface_link wam2/hand/finger_1/prox_link wam2/hand/finger_1/med_link wam2/hand/finger_1/dist_link wam2/hand/finger_2/prox_link wam2/hand/finger_2/med_link wam2/hand/finger_2/dist_link wam2/hand/finger_3/med_link wam2/hand/finger_3/dist_link wam2/wrist_palm_stump_link
      world peg1/base_link peg1/peg_link peg1/peg_top_link
      world peg2/base_link peg2/peg_link peg2/peg_top_link
      ring1/ring_link
      world stage_link
   */
  void PredicateContext::addGeometryPredicates(PredicateList &list, std::vector<double> &heuristic, const std::vector<RobotState *> &states) {

    for(typename std::vector<RobotState *>::const_iterator it = states.begin();
        it != states.end();
        ++it)
    {

      // get the list of joints for the robot state
      for (typename std::vector<std::string>::const_iterator link1 = (*it)->getRobotModel()->getLinkModelNames().begin();
           link1 != (*it)->getRobotModel()->getLinkModelNames().end();
           ++link1)
      {
        if (verbosity > 3) {
          std::cout << *link1 << " ";
        }

        if link1->compare(std::string("world")) {
          continue;
        }
      }

      if (verbosity > 3) {
        std::cout << std::endl;
      }
    }
  }
}
<commit_msg>adding geometry loop to predicator planning<commit_after>#include "predicator.h"

/**
  predicator_planning
  Can generate the set of predicates that would be true for a given location.
  Also used to generate predicates based on current state.
 **/
namespace predicator_planning {

  /*
   * createStatement()
   * Simple helper function to create predicates with
   */
  PredicateStatement createStatement(std::string predicate, double value, std::string param1, std::string param2, std::string param3 = "") {
    PredicateStatement ps;
    ps.predicate = predicate;
    ps.params[0] = param1;
    ps.params[1] = param2;
    ps.value = value;

    int num_params = 2;

    if (param3.size() > 0) {
      ++num_params;
      ps.params[2] = param3;
    }
    ps.num_params = num_params;

    return ps;
  }

  /*
   * joint_state_callback()
   * Update the robot state variable values
   */
  void joint_state_callback(const sensor_msgs::JointState::ConstPtr &msg, RobotState *state) {
    state->setVariableValues(*msg);
  }

  PredicateContext::PredicateContext(bool publish) {
    ros::NodeHandle nh_tilde("~");
    ros::NodeHandle nh;

    XmlRpc::XmlRpcValue frames_list;
    XmlRpc::XmlRpcValue descriptions;
    XmlRpc::XmlRpcValue topics;
    XmlRpc::XmlRpcValue floating; // set of floating root joints that need to be updated

    nh_tilde.param("verbosity", verbosity, 0);
    nh_tilde.param("padding", padding, 0.01);
    nh_tilde.param("world_frame", world_frame, std::string("/world"));

    // should we publish predicate messages?
    // or what?
    if(publish == true) {
      pub = nh.advertise<predicator_msgs::PredicateList>("/predicator/input", 1000);
      vpub = nh.advertise<predicator_msgs::ValidPredicates>("/predicator/valid_input", 1000);
    }

    if(nh_tilde.hasParam("description_list")) {
      nh_tilde.param("description_list", descriptions, descriptions);
    } else {
      ROS_ERROR("No list of robot description parameters!");
      exit(-1);
    }

    if(nh_tilde.hasParam("frames")) {
      nh_tilde.param("frames", frames_list, frames_list);
    } else {
      ROS_ERROR("No list of frames for geometryic predicates!");
      exit(-1);
    }

    if(nh_tilde.hasParam("joint_state_topic_list")) {
      nh_tilde.param("joint_state_topic_list", topics, topics);
    } else {
      ROS_ERROR("No list of joint state topics!");
      exit(-1);
    }

    bool load_floating = false;
    if(nh_tilde.hasParam("floating_root_list")) {
      nh_tilde.param("floating_root_list", floating, floating);
      load_floating = true;
    } else {
      ROS_INFO("No list of robots with floating root joints given.");
    }

    if(descriptions.size() != topics.size()) {
      ROS_WARN("An unequal number of joint state and robot topics was provided!");
    }

    // define valid predicates topic
    pval.predicates.push_back("touching");
    pval.value_predicates.push_back("mesh_distance");
    pval.pheader.source = ros::this_node::getName();

    // read in topics and descriptions
    for(unsigned int i = 0; i < descriptions.size(); ++i) {
      std::string desc;
      std::string topic;

      if(descriptions[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        desc = static_cast<std::string>(descriptions[i]);
        if(verbosity > 0) {
          std::cout << "Robot Description parameter name: " << desc << std::endl;
        }
      } else {
        ROS_WARN("Description %u was not of type \"string\"!", i);
        continue;
      }

      // create a robot model with state desc
      robot_model_loader::RobotModelLoader robot_model_loader(desc);
      robot_model::RobotModelPtr model = robot_model_loader.getModel();
      PlanningScene *scene = new PlanningScene(model);
      scene->getCollisionRobotNonConst()->setPadding(padding);
      scene->propogateRobotPadding();

      // get all link names as possible assignments
      for(typename std::vector<std::string>::const_iterator it = model->getLinkModelNames().begin();
          it != model->getLinkModelNames().end();
          ++it)
      {
        pval.assignments.push_back(*it);
      }

      robots.push_back(model);
      scenes.push_back(scene);

      RobotState *state = new RobotState(model);
      states.push_back(state);

      if(i < topics.size() && topics[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        topic = static_cast<std::string>(topics[i]);
        if(verbosity > 0) {
          std::cout << "JointState topic name: " << topic << std::endl;
        }

        // create the subscriber
        subs.push_back(nh.subscribe<sensor_msgs::JointState>
                       (topic, 1000,
                        boost::bind(joint_state_callback, _1, state)));
      } else if (verbosity > 0) {
        ROS_WARN("no topic corresponding to description %s!", desc.c_str());
      }
    }

    // read in frames of interest
    for(unsigned int i = 0; i < frames_list.size(); ++i) {
      std::string frame;

      if(frames_list[i].getType() == XmlRpc::XmlRpcValue::TypeString) {
        frame = static_cast<std::string>(frames_list[i]);
        if(verbosity > 0) {
          std::cout << "Including frame: " << frame << std::endl;
        }
        frames.push_back(frame);
      } else {
        ROS_WARN("Frame list entry %u was not of type \"string\"!", i);
        continue;
      }
    }

    ROS_INFO("about to parse floating");
    if(load_floating) {
      // read in root TF frames
      for(unsigned int i = 0; i < floating.size(); ++i) {
        std::string id = floating[i]["id"];
        std::string frame = floating[i]["frame"];

        floating_frames[id] = frame;
      }
    }
    ROS_INFO("parsed floating");

    // print out information on all the different joints
    if(verbosity > 0) {
      unsigned int i = 0;
      for(typename std::vector<PlanningScene *>::iterator it1 = scenes.begin();
          it1 != scenes.end();
          ++it1, ++i)
      {
        collision_detection::CollisionRobotConstPtr robot1 = (*it1)->getCollisionRobot();
        // -----------------------------------------------------------
        std::cout << std::endl;
        std::cout << "PRINTING STATE INFO:";
        std::cout << robot1->getRobotModel()->getName() << std::endl;
        std::cout << robots[i]->getRootJointName() << std::endl;
        states[i]->update(true);
        states[i]->printStateInfo(std::cout);
        // -----------------------------------------------------------
      }
    }
  }

  /**
   * cleanup()
   * Delete memory allocated for robot states and subscribers
   */
  void PredicateContext::cleanup() {
    for (typename std::vector<RobotState *>::iterator it = states.begin();
         it != states.end();
         ++it)
    {
      delete *it;
    }

    for (typename std::vector<PlanningScene *>::iterator it = scenes.begin();
         it != scenes.end();
         ++it)
    {
      delete *it;
    }
  }

  /**
   * updatRobotStates()
   * make sure base frames are up to date
   * some objects, such as free-floating robots (aka the ring) need to be updated by TF
   * not sure why this doesn't work naturally
   */
  void PredicateContext::updateRobotStates() {
    unsigned int i = 0;

    for(typename std::vector<PlanningScene *>::iterator it1 = scenes.begin();
        it1 != scenes.end();
        ++it1, ++i)
    {

      collision_detection::CollisionRobotConstPtr robot1 = (*it1)->getCollisionRobot();
      std::string name = robot1->getRobotModel()->getName();

      if(floating_frames.find(name) != floating_frames.end()) {
        std::string base_frame = floating_frames[name];

        tf::StampedTransform transform;
        Eigen::Affine3d t;

        try{
          listener.lookupTransform(world_frame, base_frame,
                                   ros::Time(0), transform);
          tf::transformTFToEigen(transform,t);
          states[i]->setJointPositions(robot1->getRobotModel()->getRootJointName(), t);
        }
        catch (tf::TransformException ex){
          ROS_ERROR("%s",ex.what());
        }

        if(verbosity > 1) {
          std::cout << "----------------------------" << std::endl;
          std::cout << "PRINTING STATE INFO:";
          std::cout << robot1->getRobotModel()->getName() << std::endl;
          std::cout << robots[i]->getRootJointName() << std::endl;
          states[i]->update(true);
          states[i]->printStateInfo(std::cout);
        }

      } else {
        continue;
      }
    }
  }

  /**
   * addCollisionPredicates()
   * main collision checking loop
   * checks for all pairs of objects, determines collisions and distances
   * publishes the relationships between all of these objects
   */
  void PredicateContext::addCollisionPredicates(PredicateList &output, std::vector<double> &heuristics, const std::vector<RobotState *> &states) {

    unsigned i = 0;
    for(typename std::vector<PlanningScene *>::iterator it1 = scenes.begin();
        it1 != scenes.end();
        ++it1, ++i)
    {

      collision_detection::CollisionRobotConstPtr robot1 = (*it1)->getCollisionRobot();

      typename std::vector<PlanningScene *>::iterator it2 = it1;
      unsigned int j = i+1;
      for(++it2; it2 != scenes.end(); ++it2, ++j) {

        //if (i == j) continue;

        collision_detection::CollisionRobotConstPtr robot2 = (*it2)->getCollisionRobot();

        collision_detection::CollisionRequest req;
        collision_detection::CollisionResult res;
        req.contacts = true;
        req.max_contacts = 1000;

        // force an update
        // source: https://groups.google.com/forum/#!topic/moveit-users/O9CEef6sxbE
        states[i]->update(true);
        states[j]->update(true);
        robot1->checkOtherCollision(req, res, *states[i], *robot2, *states[j]);
        double dist = robot1->distanceOther(*states[i], *robot2, *states[j]);

        // write distance predicate
        predicator_msgs::PredicateStatement ps_dist;
        ps_dist.predicate = "mesh_distance";
        ps_dist.value = dist;
        ps_dist.num_params = 2;
        ps_dist.params[0] = robot1->getRobotModel()->getName();
        ps_dist.params[1] = robot2->getRobotModel()->getName();
        output.statements.push_back(ps_dist);

        // the reverse is also true, so write it as well
        predicator_msgs::PredicateStatement ps_dist2;
        ps_dist2.predicate = "mesh_distance";
        ps_dist2.value = dist;
        ps_dist2.num_params = 2;
        ps_dist2.params[0] = robot1->getRobotModel()->getName();
        ps_dist2.params[1] = robot2->getRobotModel()->getName();
        output.statements.push_back(ps_dist2);

        // iterate over all collisions
        for(collision_detection::CollisionResult::ContactMap::const_iterator cit = res.contacts.begin();
            cit != res.contacts.end();
            ++cit)
        {
          // write the correct predicate
          predicator_msgs::PredicateStatement ps;
          ps.predicate = "touching";
          ps.value = 1.0;
          ps.num_params = 2;
          ps.params[0] = cit->first.first;
          ps.params[1] = cit->first.second;
          output.statements.push_back(ps);

          // the reverse is also true, so update it
          predicator_msgs::PredicateStatement ps2;
          ps2.predicate = "touching";
          ps2.value = 1.0;
          ps2.num_params = 2;
          ps2.params[0] = cit->first.second;
          ps2.params[1] = cit->first.first;
          output.statements.push_back(ps2);
        }

        if (verbosity > 1) {
          std::cout << "(" << robot1->getRobotModel()->getName()
            << ", " << robot2->getRobotModel()->getName()
            << ") : Distance to collision: " << dist << std::endl;
        }
      }
    }

  }

  /**
   * tick()
   * Run one iteration of the predicator computations
   */
  void PredicateContext::tick() {
    predicator_msgs::PredicateList output;
    output.pheader.source = ros::this_node::getName();

    std::vector<double> heuristics;

    updateRobotStates();
    addCollisionPredicates(output, heuristics, states);
    addGeometryPredicates(output, heuristics, states);
    addReachabilityPredicates(output, heuristics, states);

    pub.publish(output);
    vpub.publish(pval);
  }

  /**
   * addReachabilityPredicates()
   * compute whether or not we can reach certain points or waypoints
   */
  void PredicateContext::addReachabilityPredicates(PredicateList &list, std::vector<double> &heuristics, const std::vector<RobotState *> &states) {

  }

  /**
   * addGeometryPredicates()
   * compute the set of geometry predicates
   *
   * Links for the different world objects from the peg demo:
   world wam/base_link wam/shoulder_yaw_link wam/shoulder_pitch_link wam/upper_arm_link wam/forearm_link wam/wrist_yaw_link wam/wrist_pitch_link wam/wrist_palm_link wam/hand/bhand_palm_link wam/hand/bhand_grasp_link wam/hand/bhand_palm_surface_link wam/hand/finger_1/prox_link wam/hand/finger_1/med_link wam/hand/finger_1/dist_link wam/hand/finger_2/prox_link wam/hand/finger_2/med_link wam/hand/finger_2/dist_link wam/hand/finger_3/med_link wam/hand/finger_3/dist_link wam/wrist_palm_stump_link
   world wam2/base_link wam2/shoulder_yaw_link wam2/shoulder_pitch_link wam2/upper_arm_link wam2/forearm_link wam2/wrist_yaw_link wam2/wrist_pitch_link wam2/wrist_palm_link wam2/hand/bhand_palm_link wam2/hand/bhand_grasp_link wam2/hand/bhand_palm_surface_link wam2/hand/finger_1/prox_link wam2/hand/finger_1/med_link wam2/hand/finger_1/dist_link wam2/hand/finger_2/prox_link wam2/hand/finger_2/med_link wam2/hand/finger_2/dist_link wam2/hand/finger_3/med_link wam2/hand/finger_3/dist_link wam2/wrist_palm_stump_link
   world peg1/base_link peg1/peg_link peg1/peg_top_link
   world peg2/base_link peg2/peg_link peg2/peg_top_link
   ring1/ring_link
   world stage_link
   */
  void PredicateContext::addGeometryPredicates(PredicateList &list, std::vector<double> &heuristic, const std::vector<RobotState *> &states) {

    unsigned int i = 0;
    for(typename std::vector<RobotState *>::const_iterator it = states.begin();
        it != states.end();
        ++it, ++i)
    {

      // get the list of joints for the robot state
      for (typename std::vector<std::string>::const_iterator link1 = (*it)->getRobotModel()->getLinkModelNames().begin();
           link1 != (*it)->getRobotModel()->getLinkModelNames().end();
           ++link1)
      {
        if (verbosity > 3) {
          std::cout << *link1 << " ";
        }

        if link1->compare(std::string("world")) {
          continue;
        }

        // loop over the other objects in the world

        unsigned int j = 0;
        for(typename std::vector<RobotState *>::const_iterator it2 = states.begin();
            it2 != states.end();
            ++it2, ++j)
        {
          if (i == j) {
            continue;
          }

          // loop over the non-world links of this object
        }

      }

      if (verbosity > 3) {
        std::cout << std::endl;
      }
    }
  }
}
<|endoftext|>