#ifndef COMMON_OBSERVER_HELPERS
#define COMMON_OBSERVER_HELPERS

#include <vector>
#include <fstream>

#include "constants.h"

std::vector<int> OH_ROBOT_IDS;

int OH_getNumOfRobots() {
	return OH_ROBOT_IDS.size();
}

/**
 * Get index of a particular robot id
 */
int OH_getRobotVectorIndex(int robotId_) {
	for (uint i=0; i<OH_ROBOT_IDS.size(); i++) {
		if (OH_ROBOT_IDS[i] == robotId_) {
			return i;
		}
	}
	return -1;
}

/**
 * Register a particular robot id for robot vectors etc. Should be called e.g. when parameter file is read
 */
void OH_registerRobotId(int id_) {
	OH_ROBOT_IDS.push_back(id_);
}

/**
 * Process parameter file to set all variables stored in this file.
 * Should be called by every publisher / subscriber that communicates with robots in any way.
 */
void OH_processParameterFile() {
	std::cout << "------ OH PARAMETERS ------" << std::endl;

	std::ifstream file (PARAMETER_FILE);
	std::string line;

	if (file.is_open()) {
		int paramVal;
		std::string paramName;

		while ( std::getline (file,line)) {
			std::stringstream linestream(line);

			linestream >> paramName >> paramVal;

			if (paramName == PARAM_ROBOT_ID) {
				OH_registerRobotId(paramVal);
			}
		}
	} else {
		std::cout << "!!!! Unable to open file" << std::endl;
	}
	file.close();

	std::cout << "Robot ids: ";
	for (uint i=0; i<OH_ROBOT_IDS.size(); i++) {
		std::cout << OH_ROBOT_IDS[i] << " ";
	}
	std::cout << std::endl;
	std::cout << "---------------------------" << std::endl;
}




#endif
