<commit_before>/*
* vrpn_logger.cpp
* Log data streamed over vrpn.
* Written by: <NAME>, <EMAIL>
* University of Auckland, 2015
*/

#include "vrpn_Tracker.h"

#include "tracked_object.hpp"
#include "quat_ez.hpp"

#include <iostream>
#include <fstream>
#include <ctime>
#include <string>
#include <thread>
#include <vector>
#include <ctime>


using namespace std;

// -----------------------------------------------------------------------------
// --------------------------- COMMAND LINE ARGUMENTS --------------------------
// -----------------------------------------------------------------------------

string TAG_IP_ADDRESS = "-ip=";
string TAG_FILE_NAME =  "-filename=";
string TAG_SEPARATOR =  "-separator=";
string TAG_OBJECT_NAMES = "-objectnames=";

// -----------------------------------------------------------------------------
// ------------------------------- GLOBAL VARIABLES ----------------------------
// -----------------------------------------------------------------------------

bool stop_thread = false;

vector<tracked_object*> objects;
vector<vrpn_Tracker_Remote*> vrpn_trackers;

// -----------------------------------------------------------------------------
// ----------------------------------- STRUCTS ---------------------------------
// -----------------------------------------------------------------------------

struct tracking_info
{
	vector<string> object_names;
	string ip_address;
	string logging_filename;
	string logging_extension;
	string separator;
};

// -----------------------------------------------------------------------------
// ----------------------------- FUNCTION PROTOTYPES ---------------------------
// -----------------------------------------------------------------------------

void save_data(tracking_info tracking_setup);
int parse_inputs(int argc, char* argv[], tracking_info* tracking_setup);
void VRPN_CALLBACK handle_tracker(void* object_pointer, const vrpn_TRACKERCB t);
void logging_thread(tracking_info& tracking_setup);

// -----------------------------------------------------------------------------
// ------------------------------------ MAIN -----------------------------------
// -----------------------------------------------------------------------------

int main(int argc, char* argv[])
{
	tracking_info tracking_setup;

	int valid_input = parse_inputs(argc, argv, &tracking_setup);

	if (valid_input != 0)
	{
		thread log_t(logging_thread, ref(tracking_setup));

		// Wait for user to press the enter key
		cin.get();

		stop_thread = true;

		// Wait for thread to close
		log_t.join();
	}

	return 0;
}

// -----------------------------------------------------------------------------
// ---------------------------------- FUNCTIONS --------------------------------
// -----------------------------------------------------------------------------

void VRPN_CALLBACK handle_tracker(void* object_pointer, const vrpn_TRACKERCB t)
{
	rotation_quat tracked_quat;
	tracked_quat.qx = t.quat[0];
	tracked_quat.qy = t.quat[1];
	tracked_quat.qz = t.quat[2];
	tracked_quat.qw = t.quat[3];

	position tracked_pos;
	tracked_pos.x = t.pos[0];
	tracked_pos.y = t.pos[1];
	tracked_pos.z = t.pos[2];

	tracked_object* object = (tracked_object*)object_pointer;
	object->update_pose(tracked_pos, tracked_quat, t.msg_time.tv_sec, t.msg_time.tv_usec);
}

void logging_thread(tracking_info& tracking_setup)
{

	for (int i = 0; i < (int)tracking_setup.object_names.size(); i++)
	{
		objects.push_back(new tracked_object(tracking_setup.object_names.at(i)));
	}

	// Initiate callback for each object
	
	for (int i = 0; i < (int)objects.size(); i++)
	{
		string vrpn_address = tracking_setup.object_names.at(i) + "@" + tracking_setup.ip_address;

		vrpn_trackers.push_back(new vrpn_Tracker_Remote(vrpn_address.c_str()));

		vrpn_trackers.at(i)->register_change_handler((void*)objects.at(i), handle_tracker);
	}

	while (!stop_thread)
	{
		for (int i = 0; i < (int)vrpn_trackers.size(); i++)
		{
			vrpn_trackers.at(i)->mainloop();
		}
	}

	save_data(tracking_setup);
}

void save_data(tracking_info tracking_setup)
{
	// Log each object as filename_objectname.extension
	string filename = tracking_setup.logging_filename + "." + tracking_setup.logging_extension;
	string sep = tracking_setup.separator;
	ofstream logging_file;
	logging_file.open(filename.c_str());


	// Find object with the most saved data
	int max_data_index = 0;
	for (int i = 1; i < (int)objects.size(); i++)
	{
		if (objects.at(i)->size() > objects.at(max_data_index)->size())
		{
			max_data_index = i;
		}
	}

	// If saving to csv, add tag to ensure excel import compatibility
	if (tracking_setup.logging_extension.compare("csv") == 0)
	{
		logging_file << "sep = " << tracking_setup.separator << endl;
	}

	// Create headers
	int max_data_length = objects.at(max_data_index)->size();

	for (int j = 0; j < (int)objects.size(); j++)
	{
		string name = objects.at(j)->get_name();
		logging_file << "time_" << name << sep << "x_" << name << sep << "y_" << name << sep <<
			"z_" << name << sep << "roll_" << name << sep << "pitch_" << name << sep << "yaw_" <<
			name << sep;
	}
	logging_file << endl;

	// Save data
	for (int i = 0; i < max_data_length; i++)
	{
		for (int j = 0; j < (int)objects.size(); j++)
		{
			if (i < (int)objects.at(j)->size())
			{
				logging_file << objects.at(j)->get_time(i) << sep <<
					objects.at(j)->get_position(i).x << sep <<
					objects.at(j)->get_position(i).y << sep <<
					objects.at(j)->get_position(i).z << sep <<
					objects.at(j)->get_euler(i).roll << sep <<
					objects.at(j)->get_euler(i).pitch << sep <<
					objects.at(j)->get_euler(i).yaw << sep;
			}
			else
			{
				// There's no more data to save for this object
				logging_file << sep << sep << sep << sep << sep << sep << sep;
			}
		}
		logging_file << endl;
	}

	logging_file.close();
}

int parse_inputs(int argc, char* argv[], tracking_info* tracking_setup)
{

	if (argc < 1)
	{
		// Needs at least the tracked object's name
		cout << "Err: not enough input arguments\n";
		cout << "Err: example syntax: vrpn_logger -ip=192.168.20.4 -filename=data.csv "
			<< "-objectnames=quadcopter0,quadcopter1\n";

		return 0;
	}
	else
	{
		// Default settings
		// Generate filename in format recording_yyyy_mm_dd_hh_mm.txt
		time_t t = time(0);
		struct tm * now = localtime(&t);

		string year = to_string(now->tm_year + 1900);
		string month = to_string(now->tm_mon + 1);
		string day = to_string(now->tm_mday);
		string hours = to_string(now->tm_hour);
		string minutes = to_string(now->tm_min);

		tracking_setup->logging_filename = "recording_" + year + "_" + month + "_" +
			day + "_" + hours + "_" + minutes;
		tracking_setup->logging_extension = "csv";
		tracking_setup->separator = ',';
		tracking_setup->ip_address = "localhost";		

		bool found_object_name = false;

		for (int i = 1; i < argc; i++)
		{
			string argument = argv[i];

			// Check which tag the user entered
			if (argument.compare(0, TAG_IP_ADDRESS.size(), TAG_IP_ADDRESS) == 0)
			{
				// Check validity of ip address
				string ip_address_str = argument.substr(TAG_IP_ADDRESS.size());<commit_msg>Add messages to show what the program is doing<commit_after>/*
* vrpn_logger.cpp
* Log data streamed over vrpn.
* Written by: <NAME>, <EMAIL>
* University of Auckland, 2015
*/

#include "vrpn_Tracker.h"

#include "tracked_object.hpp"
#include "quat_ez.hpp"

#include <iostream>
#include <fstream>
#include <ctime>
#include <string>
#include <thread>
#include <vector>
#include <ctime>


using namespace std;

// -----------------------------------------------------------------------------
// --------------------------- COMMAND LINE ARGUMENTS --------------------------
// -----------------------------------------------------------------------------

string TAG_IP_ADDRESS = "-ip=";
string TAG_FILE_NAME =  "-filename=";
string TAG_SEPARATOR =  "-separator=";
string TAG_OBJECT_NAMES = "-objectnames=";

// -----------------------------------------------------------------------------
// ------------------------------- GLOBAL VARIABLES ----------------------------
// -----------------------------------------------------------------------------

bool stop_thread = false;

vector<tracked_object*> objects;
vector<vrpn_Tracker_Remote*> vrpn_trackers;

// -----------------------------------------------------------------------------
// ----------------------------------- STRUCTS ---------------------------------
// -----------------------------------------------------------------------------

struct tracking_info
{
	vector<string> object_names;
	string ip_address;
	string logging_filename;
	string logging_extension;
	string separator;
};

// -----------------------------------------------------------------------------
// ----------------------------- FUNCTION PROTOTYPES ---------------------------
// -----------------------------------------------------------------------------

void save_data(tracking_info tracking_setup);
int parse_inputs(int argc, char* argv[], tracking_info* tracking_setup);
void VRPN_CALLBACK handle_tracker(void* object_pointer, const vrpn_TRACKERCB t);
void logging_thread(tracking_info& tracking_setup);

// -----------------------------------------------------------------------------
// ------------------------------------ MAIN -----------------------------------
// -----------------------------------------------------------------------------

int main(int argc, char* argv[])
{
	tracking_info tracking_setup;

	int valid_input = parse_inputs(argc, argv, &tracking_setup);

	if (valid_input != 0)
	{
		thread log_t(logging_thread, ref(tracking_setup));

		// Wait for user to press the enter key
		cin.get();

		stop_thread = true;

		// Wait for thread to close
		log_t.join();
	}

	return 0;
}

// -----------------------------------------------------------------------------
// ---------------------------------- FUNCTIONS --------------------------------
// -----------------------------------------------------------------------------

void VRPN_CALLBACK handle_tracker(void* object_pointer, const vrpn_TRACKERCB t)
{
	rotation_quat tracked_quat;
	tracked_quat.qx = t.quat[0];
	tracked_quat.qy = t.quat[1];
	tracked_quat.qz = t.quat[2];
	tracked_quat.qw = t.quat[3];

	position tracked_pos;
	tracked_pos.x = t.pos[0];
	tracked_pos.y = t.pos[1];
	tracked_pos.z = t.pos[2];

	tracked_object* object = (tracked_object*)object_pointer;
	object->update_pose(tracked_pos, tracked_quat, t.msg_time.tv_sec, t.msg_time.tv_usec);
}

void logging_thread(tracking_info& tracking_setup)
{

	for (int i = 0; i < (int)tracking_setup.object_names.size(); i++)
	{
		objects.push_back(new tracked_object(tracking_setup.object_names.at(i)));
	}

	// Initiate callback for each object

	for (int i = 0; i < (int)objects.size(); i++)
	{
		string vrpn_address = tracking_setup.object_names.at(i) + "@" + tracking_setup.ip_address;

		vrpn_trackers.push_back(new vrpn_Tracker_Remote(vrpn_address.c_str()));

		vrpn_trackers.at(i)->register_change_handler((void*)objects.at(i), handle_tracker);
	}

	cout << endl << "Recording data... Press [Enter] to stop and save data " <<
		"to the disc." << endl << endl;
	
	while (!stop_thread)
	{
		for (int i = 0; i < (int)vrpn_trackers.size(); i++)
		{
			vrpn_trackers.at(i)->mainloop();
		}
	}

	save_data(tracking_setup);
}

void save_data(tracking_info tracking_setup)
{
	// Log each object as filename_objectname.extension
	string filename = tracking_setup.logging_filename + "." + tracking_setup.logging_extension;
	string sep = tracking_setup.separator;
	ofstream logging_file;
	logging_file.open(filename.c_str());

	cout << endl << "Saving data to " << filename << "..." << endl;

	// Find object with the most saved data
	int max_data_index = 0;
	for (int i = 1; i < (int)objects.size(); i++)
	{
		if (objects.at(i)->size() > objects.at(max_data_index)->size())
		{
			max_data_index = i;
		}
	}

	// If saving to csv, add tag to ensure excel import compatibility
	if (tracking_setup.logging_extension.compare("csv") == 0)
	{
		logging_file << "sep = " << tracking_setup.separator << endl;
	}

	// Create headers
	int max_data_length = objects.at(max_data_index)->size();

	for (int j = 0; j < (int)objects.size(); j++)
	{
		string name = objects.at(j)->get_name();
		logging_file << "time_" << name << sep << "x_" << name << sep << "y_" << name << sep <<
			"z_" << name << sep << "roll_" << name << sep << "pitch_" << name << sep << "yaw_" <<
			name << sep;
	}
	logging_file << endl;

	// Save data
	for (int i = 0; i < max_data_length; i++)
	{
		for (int j = 0; j < (int)objects.size(); j++)
		{
			if (i < (int)objects.at(j)->size())
			{
				logging_file << objects.at(j)->get_time(i) << sep <<
					objects.at(j)->get_position(i).x << sep <<
					objects.at(j)->get_position(i).y << sep <<
					objects.at(j)->get_position(i).z << sep <<
					objects.at(j)->get_euler(i).roll << sep <<
					objects.at(j)->get_euler(i).pitch << sep <<
					objects.at(j)->get_euler(i).yaw << sep;
			}
			else
			{
				// There's no more data to save for this object
				logging_file << sep << sep << sep << sep << sep << sep << sep;
			}
		}
		logging_file << endl;
	}

	logging_file.close();

	cout << endl << "Data saved" << endl << endl;
}

int parse_inputs(int argc, char* argv[], tracking_info* tracking_setup)
{

	if (argc < 1)
	{
		// Needs at least the tracked object's name
		cout << "Err: not enough input arguments\n";
		cout << "Err: example syntax: vrpn_logger -ip=192.168.20.4 -filename=data.csv "
			<< "-objectnames=quadcopter0,quadcopter1\n";

		return 0;
	}
	else
	{
		// Default settings
		// Generate filename in format recording_yyyy_mm_dd_hh_mm.txt
		time_t t = time(0);
		struct tm * now = localtime(&t);

		string year = to_string(now->tm_year + 1900);
		string month = to_string(now->tm_mon + 1);
		string day = to_string(now->tm_mday);
		string hours = to_string(now->tm_hour);
		string minutes = to_string(now->tm_min);

		if (stoi(month)   < 10) { month   = "0" + month; }
		if (stoi(day)     < 10) { day     = "0" + day; }
		if (stoi(hours)   < 10) { hours   = "0" + hours; }
		if (stoi(minutes) < 10) { minutes = "0" + minutes; }

		tracking_setup->logging_filename = "recording_" + year + "_" + month + "_" +
			day + "_" + hours + "_" + minutes;
		tracking_setup->logging_extension = "csv";
		tracking_setup->separator = ',';
		tracking_setup->ip_address = "localhost";		

		bool found_object_name = false;

		for (int i = 1; i < argc; i++)
		{
			string argument = argv[i];

			// Check which tag the user entered
			if (argument.compare(0, TAG_IP_ADDRESS.size(), TAG_IP_ADDRESS) == 0)
			{
				// Check validity of ip address
				string ip_address_str = argument.substr(TAG_IP_ADDRESS.size());
{'fraction_non_alphanumeric': 0.3050064533659398, 'fraction_numerical': 0.006793016778751443, 'mean_word_length': 1.879890453834116, 'pattern_counts': {'":': 0, '<': 279, '<?xml version=': 0, '>': 77, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 2, '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': '3549728', 'n_tokens_mistral': 4942, 'n_tokens_neox': 4487, 'n_words': 1004}