<commit_before>// Copyright 2017-2020, Institute for Artificial Intelligence - University of Bremen
// Author: <NAME> (http://haidu.eu)

#include "Runtime/SLWorldStateDBHandler.h"
#include "Individuals/SLIndividualManager.h"

#include "Individuals/Type/SLBaseIndividual.h"
#include "Individuals/Type/SLSkeletalIndividual.h"
#include "Individuals/Type/SLBoneIndividual.h"
#include "Individuals/Type/SLVirtualBoneIndividual.h"
#include "Individuals/Type/SLRobotIndividual.h"

// UUtils
#if SL_WITH_ROS_CONVERSIONS
#include "Conversions.h"
#endif // SL_WITH_ROS_CONVERSIONS

/* DB Write Async Task */
// Init task
#if SL_WITH_LIBMONGO_C
bool FSLWorldStateDBWriterAsyncTask::Init(mongoc_collection_t* in_collection, ASLIndividualManager* Manager, float PoseTolerance, bool bInWriteSparse)
{
	IndividualManager = Manager;
	mongo_collection = in_collection;
	MinPoseDiff = PoseTolerance;
	bWriteSparse = bInWriteSparse;

	// Set the write function pointer (first write is without optimization, write all individuals)
	WriteFunctionPtr = &FSLWorldStateDBWriterAsyncTask::FirstWrite;

	return true;
}
#endif //SL_WITH_LIBMONGO_C	

// Do the db writing here
void FSLWorldStateDBWriterAsyncTask::DoWork()
{
	const double StartTime = FPlatformTime::Seconds();

	// Call the write function pointer
	int32 NumEntries = (this->*WriteFunctionPtr)();

	//double Duration = FPlatformTime::Seconds() - StartTime;
	//UE_LOG(LogTemp, Warning, TEXT("%s::%d \t\t\t Async work (written %ld entries) duration:\t%f (s)"),
	//	*FString(__FUNCTION__), __LINE__, NumEntries, Duration);
}

// First write where all the individuals are written irregardresly of their previous position
int32 FSLWorldStateDBWriterAsyncTask::FirstWrite()
{
	// Count the number of entries written to the document (if 0, skip upload)
	int32 Num = 0;

#if SL_WITH_LIBMONGO_C
	bson_t* ws_doc;
	ws_doc = bson_new();

	AddTimestamp(ws_doc);

	Num += AddAllIndividuals(ws_doc);
	Num += AddSkeletalIndividals(ws_doc);
	//Num += AddRobotIndividuals(ws_doc);

	// Write only if there are any entries in the document
	if (Num > 0)
	{
		UploadDoc(ws_doc);
	}

	// Clean up
	bson_destroy(ws_doc);
#endif //SL_WITH_LIBMONGO_C	

	// Change the write function pointer to write only individuals that are moving
	if (bWriteSparse)
	{
		WriteFunctionPtr = &FSLWorldStateDBWriterAsyncTask::WriteSparse;
	}
	else
	{
		WriteFunctionPtr = &FSLWorldStateDBWriterAsyncTask::WriteAll;
	}

	return Num;
}

// Write only the indviduals that changed pose
int32 FSLWorldStateDBWriterAsyncTask::WriteSparse()
{
	// Count the number of entries written to the document (if 0, skip upload)
	int32 Num = 0;

#if SL_WITH_LIBMONGO_C
	bson_t* ws_doc;
	ws_doc = bson_new();

	AddTimestamp(ws_doc);

	Num += AddIndividualsThatMoved(ws_doc);
	Num += AddSkeletalIndividals(ws_doc);
	//Num += AddRobotIndividuals(ws_doc);

	// Write only if there are any entries in the document
	if (Num > 0)
	{
		UploadDoc(ws_doc);
	}

	// Clean up
	bson_destroy(ws_doc);
#endif //SL_WITH_LIBMONGO_C

	return Num;
}

// Write all individuals
int32 FSLWorldStateDBWriterAsyncTask::WriteAll()
{
	// Count the number of entries written to the document (if 0, skip upload)
	int32 Num = 0;

#if SL_WITH_LIBMONGO_C
	bson_t* ws_doc;
	ws_doc = bson_new();

	AddTimestamp(ws_doc);

	Num += AddAllIndividuals(ws_doc);
	Num += AddSkeletalIndividals(ws_doc);
	//Num += AddRobotIndividuals(ws_doc);

	// Write only if there are any entries in the document
	if (Num > 0)
	{
		UploadDoc(ws_doc);
	}

	// Clean up
	bson_destroy(ws_doc);
#endif //SL_WITH_LIBMONGO_C

	return Num;
}

#if SL_WITH_LIBMONGO_C
// Add timestamp to the bson doc
void FSLWorldStateDBWriterAsyncTask::AddTimestamp(bson_t* doc)
{
	BSON_APPEND_DOUBLE(doc, "timestamp", Timestamp);
}

// Add all individuals (return the number of individuals added)
int32 FSLWorldStateDBWriterAsyncTask::AddAllIndividuals(bson_t* doc)
{
	int32 Num = 0;
	bson_t arr_obj;
	uint32_t arr_idx = 0;

	BSON_APPEND_ARRAY_BEGIN(doc, "individuals", &arr_obj);
	for (const auto& Individual : IndividualManager->GetIndividuals())
	{
		//Individual->UpdateCachedPose(0.0);

		//// TODO workaround
		//Individual->SetHasMovedFlag(true);

		bson_t individual_obj;
		char idx_str[16];
		const char* idx_key;

		bson_uint32_to_string(arr_idx, &idx_key, idx_str, sizeof idx_str);
		BSON_APPEND_DOCUMENT_BEGIN(&arr_obj, idx_key, &individual_obj);
			// Id
			BSON_APPEND_UTF8(&individual_obj, "id", TCHAR_TO_UTF8(*Individual->GetIdValue()));			
			// Pose
			AddPose(Individual->GetCachedPose(), &individual_obj);
		bson_append_document_end(&arr_obj, &individual_obj);

		arr_idx++;
		Num++;<commit_msg>raw world logger bug fix<commit_after>// Copyright 2017-2020, Institute for Artificial Intelligence - University of Bremen
// Author: <NAME> (http://haidu.eu)

#include "Runtime/SLWorldStateDBHandler.h"
#include "Individuals/SLIndividualManager.h"

#include "Individuals/Type/SLBaseIndividual.h"
#include "Individuals/Type/SLSkeletalIndividual.h"
#include "Individuals/Type/SLBoneIndividual.h"
#include "Individuals/Type/SLVirtualBoneIndividual.h"
#include "Individuals/Type/SLRobotIndividual.h"

// UUtils
#if SL_WITH_ROS_CONVERSIONS
#include "Conversions.h"
#endif // SL_WITH_ROS_CONVERSIONS

/* DB Write Async Task */
// Init task
#if SL_WITH_LIBMONGO_C
bool FSLWorldStateDBWriterAsyncTask::Init(mongoc_collection_t* in_collection, ASLIndividualManager* Manager, float PoseTolerance, bool bInWriteSparse)
{
	IndividualManager = Manager;
	mongo_collection = in_collection;
	MinPoseDiff = PoseTolerance;
	bWriteSparse = bInWriteSparse;

	// Set the write function pointer (first write is without optimization, write all individuals)
	WriteFunctionPtr = &FSLWorldStateDBWriterAsyncTask::FirstWrite;

	return true;
}
#endif //SL_WITH_LIBMONGO_C	

// Do the db writing here
void FSLWorldStateDBWriterAsyncTask::DoWork()
{
	const double StartTime = FPlatformTime::Seconds();

	// Call the write function pointer
	int32 NumEntries = (this->*WriteFunctionPtr)();

	//double Duration = FPlatformTime::Seconds() - StartTime;
	//UE_LOG(LogTemp, Warning, TEXT("%s::%d \t\t\t Async work (written %ld entries) duration:\t%f (s)"),
	//	*FString(__FUNCTION__), __LINE__, NumEntries, Duration);
}

// First write where all the individuals are written irregardresly of their previous position
int32 FSLWorldStateDBWriterAsyncTask::FirstWrite()
{
	// Count the number of entries written to the document (if 0, skip upload)
	int32 Num = 0;

#if SL_WITH_LIBMONGO_C
	bson_t* ws_doc;
	ws_doc = bson_new();

	AddTimestamp(ws_doc);

	Num += AddAllIndividuals(ws_doc);
	Num += AddSkeletalIndividals(ws_doc);
	//Num += AddRobotIndividuals(ws_doc);

	// Write only if there are any entries in the document
	if (Num > 0)
	{
		UploadDoc(ws_doc);
	}

	// Clean up
	bson_destroy(ws_doc);
#endif //SL_WITH_LIBMONGO_C	

	// Change the write function pointer to write only individuals that are moving
	if (bWriteSparse)
	{
		WriteFunctionPtr = &FSLWorldStateDBWriterAsyncTask::WriteSparse;
	}
	else
	{
		WriteFunctionPtr = &FSLWorldStateDBWriterAsyncTask::WriteAll;
	}

	return Num;
}

// Write only the indviduals that changed pose
int32 FSLWorldStateDBWriterAsyncTask::WriteSparse()
{
	// Count the number of entries written to the document (if 0, skip upload)
	int32 Num = 0;

#if SL_WITH_LIBMONGO_C
	bson_t* ws_doc;
	ws_doc = bson_new();

	AddTimestamp(ws_doc);

	Num += AddIndividualsThatMoved(ws_doc);
	Num += AddSkeletalIndividals(ws_doc);
	//Num += AddRobotIndividuals(ws_doc);

	// Write only if there are any entries in the document
	if (Num > 0)
	{
		UploadDoc(ws_doc);
	}

	// Clean up
	bson_destroy(ws_doc);
#endif //SL_WITH_LIBMONGO_C

	return Num;
}

// Write all individuals
int32 FSLWorldStateDBWriterAsyncTask::WriteAll()
{
	// Count the number of entries written to the document (if 0, skip upload)
	int32 Num = 0;

#if SL_WITH_LIBMONGO_C
	bson_t* ws_doc;
	ws_doc = bson_new();

	AddTimestamp(ws_doc);

	Num += AddAllIndividuals(ws_doc);
	Num += AddSkeletalIndividals(ws_doc);
	//Num += AddRobotIndividuals(ws_doc);

	// Write only if there are any entries in the document
	if (Num > 0)
	{
		UploadDoc(ws_doc);
	}

	// Clean up
	bson_destroy(ws_doc);
#endif //SL_WITH_LIBMONGO_C

	return Num;
}

#if SL_WITH_LIBMONGO_C
// Add timestamp to the bson doc
void FSLWorldStateDBWriterAsyncTask::AddTimestamp(bson_t* doc)
{
	BSON_APPEND_DOUBLE(doc, "timestamp", Timestamp);
}

// Add all individuals (return the number of individuals added)
int32 FSLWorldStateDBWriterAsyncTask::AddAllIndividuals(bson_t* doc)
{
	int32 Num = 0;
	bson_t arr_obj;
	uint32_t arr_idx = 0;

	BSON_APPEND_ARRAY_BEGIN(doc, "individuals", &arr_obj);
	for (const auto& Individual : IndividualManager->GetIndividuals())
	{
		Individual->UpdateCachedPose(0.0);

		//// TODO workaround
		//Individual->SetHasMovedFlag(true);

		bson_t individual_obj;
		char idx_str[16];
		const char* idx_key;

		bson_uint32_to_string(arr_idx, &idx_key, idx_str, sizeof idx_str);
		BSON_APPEND_DOCUMENT_BEGIN(&arr_obj, idx_key, &individual_obj);
			// Id
			BSON_APPEND_UTF8(&individual_obj, "id", TCHAR_TO_UTF8(*Individual->GetIdValue()));			
			// Pose
			AddPose(Individual->GetCachedPose(), &individual_obj);
		bson_append_document_end(&arr_obj, &individual_obj);

		arr_idx++;
		Num++;
{'fraction_non_alphanumeric': 0.10951195758034844, 'fraction_numerical': 0.010172059300941456, 'mean_word_length': 5.181939799331103, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 23, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '10847', 'n_tokens_mistral': 3550, 'n_tokens_neox': 3235, 'n_words': 775}