#include <stdlib.h>
#include <IPCInterface.h>
#include <cpp_observer/ViconPosition.h>
#include "ros/ros.h"
#include "vector"
#include "string"
#include "sstream"

#include "../../cpp_common/constants.h"
#include "../../cpp_common/observerHelpers.h"

#include "EpuckCommSubscriberTopic.h"

int main(int argc, char **argv) {
  
  if(argc < 2){
    std::cout << "Usage : EpuckCommSubscriber PORTNUMBER" << std::endl;
    return(1);
  }

  IPCInterface Server("localhost",strtol(argv[1],NULL,0));
  Server.InitializeServer();

  ros::init(argc, argv, "EpuckCommSubscriber");

  ros::NodeHandle n;
  std::stringstream TopicName,IPAddress;
  std::vector<EpuckCommSubscriberTopic> TopicVector;

  int maxNumOfRobots = 40;
  TopicVector.resize(maxNumOfRobots);
  OH_processParameterFile();
  
  int numOfRobots = OH_getNumOfRobots();
  int robotVectorIndex, robotId;

  std::cout << "===========================================================" << std::endl;
  for(std::vector<EpuckCommSubscriberTopic>::iterator TopicIterator = TopicVector.begin(); TopicIterator != TopicVector.end(); TopicIterator++){
	  robotVectorIndex = distance(TopicVector.begin(),TopicIterator);

	  if (robotVectorIndex < numOfRobots) {
		  robotId = OH_ROBOT_IDS[robotVectorIndex];

		  IPAddress << "192.168.20." << robotId;
		  TopicIterator->SetIP(IPAddress.str());

		  TopicName << "/epuck" << robotId;
		  TopicName << "/Comm";

		  std::cout << " Setting up for " << TopicName.str() << " IP " << IPAddress.str() << std::endl;
		  TopicIterator->SetTopicName(TopicName.str());

		  TopicIterator->SubscriberTopicNode = n.subscribe(TopicIterator->GetTopicName()->c_str(),
								 1000,
								 &EpuckCommSubscriberTopic::UpdatePositionCallback,
								 &TopicVector[distance(TopicVector.begin(),TopicIterator)]);
		  IPAddress.str("");
		  TopicName.str("");
	  }
  }

  ros::Rate loop_rate(10);

  while (ros::ok()){         

	  for(std::vector<EpuckCommSubscriberTopic>::iterator TopicIterator = TopicVector.begin(); TopicIterator!=TopicVector.end(); TopicIterator++) {
		  robotVectorIndex = distance(TopicVector.begin(),TopicIterator);
		  if (robotVectorIndex < numOfRobots) {
			  Server.SendRobotCommunication(TopicIterator->GetIP()->c_str(),
					  TopicIterator->GetWorksiteId(), TopicIterator->GetWorksiteX(), TopicIterator->GetWorksiteY(), TopicIterator->GetRecruiterId()
				  );
		  }
	  }
	  ros::spinOnce();
	  loop_rate.sleep();
  }

  return 0;
}

