#include <iostream>
#include <sstream>
#include <cassert>
#include <cstring>
#include <vector>
#include <cstdlib>
#include <algorithm>
#include <functional>
#include <limits>
#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <signal.h>
#include <fstream>
#include "Vicon.h"
#include "gps.h"
#include <cpp_observer/ViconPosition.h>
#include <IPCInterface.h>
#include "ros/ros.h"

#include "Vector2D.h"

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

#define PORT 800
#define HOST "192.168.2.2"
#define MAXDATASIZE 100
#define SOCKET_ERROR -1
#define PI 3.14159265

#define DEBUG_ROBOT_MESSAGES 0
#define DEBUG_WORKSITE_PROCESSING 0
#define DEBUG_WORKSITE_VOLUME_LEFT 1
#define DEBUG_REPORT_RECEIVING 0
#define DEBUG_WORLD_GENERATION 0

// useful macros
#define DEG2RAD(x) (((double)(x))*0.01745329251994)
#define RAD2DEG(x) (((double)(x))*57.29577951308232)

//-- structs

struct WorksiteData {

	int id;
	int xPos;
	int yPos;
	double volume;
	bool isActive;

	WorksiteData(int id_, int x_, int y_, double vol_) {
		id = id_;
		xPos = x_;
		yPos = y_;
		volume = vol_;
		isActive = true;
	}
};

//-- functions
void signalHandler(int dummy);

bool recieve(int Socket, char * pBuffer, int BufferSize);
bool recieve(int Socket, int32_t & Val);
bool recieve(int Socket, uint32_t & Val);
bool recieve(int Socket, double & Val);

robot_position_t ProcessBodyData(std::vector<BodyData>::iterator iBodyData, std::vector<BodyChannel>::iterator iBody, std::vector< double > data);

void ProcessParameterFile();
void GenerateWorld();
void AddWorksite(int id_, Vector2D* position_);
WorksiteData* GetWorksiteById(int id_);

bool GetCanRobotSenseWorksite(int robotX_, int robotY_, WorksiteData worksiteData_);

//-- global vars
int user_quit;

std::ofstream outputFileInfoEvents;
//std::ofstream outputFileGlobal;

int numOfWorksites;
int minWorksiteD;
int maxWorksiteD;
int worksiteRadius;
int worksiteVolume;
int robotWorksiteSenseRange;
int baseX;
int baseY;
int baseRadius;
int missionType;

int timeSinceStart;
int timeLost;
time_t connectionErrorStartTime;
int numOfRobots;
std::vector<WorksiteData> worksites;



/* ============================= MAIIN ================================*/

int main(int argc, char **argv) {

  /* -------------------------------------------------
   *		Added by Paul O'Dowd
   * ------------------------------------------------- */

  int refreshDelay = 0;

  //-- init time
  time_t startTime = time(0);
  connectionErrorStartTime = time(0);
  timeSinceStart = 0;
  timeLost = 0;
  std::srand(time(0));

  //-- handle messages

  if (argc < 3 ) {
	  fprintf(stderr,"Usage : EpuckViconPublisher REFRESHRATE1 RECEIVE_PORTNUMBER \n");
	  exit(1);
  }
  refreshDelay = atoi(argv[1]);

  user_quit = 0;
  //set signal handler to capture "ctrl+c" event
  if (signal(SIGINT, signalHandler) == SIG_ERR) {
	  printf("signal(2) failed while setting up for SIGINT");
	  return -1;
  }

  if (signal(SIGTERM, signalHandler) == SIG_ERR) {
	  printf("signal(2) failed while setting up for SIGTERM");
	  return -1;
  }

  //-- open report files
  //outputFileGlobal.open("/home/lenka/EPUCKimage/mnt/root/taskDiscovery_epuck/cpp_observer/launch/global.txt", std::ios_base::trunc | std::ios_base::out);
 // outputFileGlobal << "time\tworkDone" << std::endl;

  outputFileInfoEvents.open("/home/lenka/EPUCKimage/mnt/root/taskDiscovery_epuck/cpp_observer/launch/infoEvents.txt", std::ios_base::trunc | std::ios_base::out);
  outputFileInfoEvents << "time\tevtType\t\trobotId\tsiteId\tsiteX\tsiteY\tsiteVol\trecruiterId\treceivedTime" << std::endl;

  std::vector<InfoEventReport> infoEventReports;

  //-- setup world parameters and worksites
  ProcessParameterFile();
  GenerateWorld();

  //std::cout << " 1 " << std::endl;
  //-- setup receiving messages from robots
  IPCInterface Server("localhost",strtol(argv[2],NULL,0));
  Server.InitializeServer();

  //std::cout << " 2 " << std::endl;
  //-- other stuff

  robot_position_t robotData; // transmission packet containing dats
  int sockfd;
  struct hostent *he;
  struct sockaddr_in their_addr;
  double totalReward = 0;
  //int lastReportedGlobalTimeSinceStart = -1;

  // get the host info
  std::cout << ">>> connecting to host " << HOST << std::endl;
  if ((he = gethostbyname(HOST)) == NULL) {
      herror("gethostbyname");
      exit(-1);
  }

  if ((sockfd = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
      perror("socket");
      exit(-1);
  }

  their_addr.sin_family = AF_INET;    // host byte order
  their_addr.sin_port = htons(PORT);    // short, network byte order
  their_addr.sin_addr = *((struct in_addr *)he->h_addr);
  memset(their_addr.sin_zero, '\0', sizeof their_addr.sin_zero);

  //connect to Vicon system

  if (connect(sockfd, (struct sockaddr *)&their_addr, sizeof their_addr) == -1) {
      perror("connect");
      exit(-1);
  }

  // A connection with the Vicon Realtime system is now open.
  // The following section implements the new Computer Graphics Client interface.

  int retrial_count=0;


  /* ------------------------------------- MAIN LOOP ----------------------------*/
  while( user_quit!=1) { //retrial_count<5 &&
      retrial_count++;
      std::cout << "retrial " << retrial_count << std::endl;
      try {



          std::vector< std::string > info;
          const int bufferSize = 2040;
          char buff[bufferSize];
          char * pBuff;

          //- Get Info - - - - - - - - - - - - - - - - - - - - - - - - - - - -
          // Request the channel information

          pBuff = buff;

          * ((int32_t *) pBuff) = ClientCodes::EInfo;
          pBuff += sizeof(int32_t);
          * ((int32_t *) pBuff) = ClientCodes::ERequest;
          pBuff += sizeof(int32_t);

          if (send(sockfd, buff, pBuff - buff, 0) == SOCKET_ERROR)
              throw std::string("Error Requesting");

          /** long int packet; */
          /** long int type; */

          int32_t packet;
          int32_t type;

          if (!recieve(sockfd, packet))
              throw std::string("Error Recieving");

          if (!recieve(sockfd, type))
              throw std::string("Error Recieving");

          if (type != ClientCodes::EReply)
              throw std::string("Bad Packet");

          if (packet != ClientCodes::EInfo)
              throw std::string("Bad Reply Type");

          uint32_t size;

          if (!recieve(sockfd, size))
              throw std::string();

          info.resize(size);

          std::vector< std::string >::iterator iInfo;

          for (iInfo = info.begin(); iInfo != info.end(); iInfo++)
          {
              int32_t s;
              char c[255];
              char * p = c;

              if (!recieve(sockfd, s))
                  throw std::string();

              if (!recieve(sockfd, c, s))
                  throw std::string();

              p += s;

              *p = 0;

              *iInfo = std::string(c);
              //std::cout << std::string(c) << std::endl;
          }

          //- Parse Info - - - - - - - - - - - - - - - - - - - - - - - - - - - -
          // The info packets now contain the channel names.
          // Identify the channels with the various dof's.

          std::vector< MarkerChannel > MarkerChannels;
          std::vector< BodyChannel > BodyChannels;
          //int FrameChannel;

          for (iInfo = info.begin(); iInfo != info.end(); iInfo++) {
              // Extract the channel type

              int openBrace = iInfo->find('<');

              if (openBrace == int(iInfo->npos))
                  throw std::string("Bad Channel Id");

              int closeBrace = iInfo->find('>');

              if (closeBrace == int(iInfo->npos))
                  throw std::string("Bad Channel Id");

              closeBrace++;

              std::string Type = iInfo->substr(openBrace, closeBrace - openBrace);

              // Extract the Name

              std::string Name = iInfo->substr(0, openBrace);

              int space = Name.rfind(' ');

              if (space != int(Name.npos))
                  Name.resize(space);

              std::vector< MarkerChannel >::iterator iMarker;
              std::vector< BodyChannel >::iterator iBody;
              std::vector< std::string >::const_iterator iTypes;

              iMarker = std::find( MarkerChannels.begin(),
                      MarkerChannels.end(), Name);

              iBody = std::find(BodyChannels.begin(), BodyChannels.end(), Name);

              if (iMarker != MarkerChannels.end()) {
                  // The channel is for a marker we already have.
                  iTypes = std::find( ClientCodes::MarkerTokens.begin(), ClientCodes::MarkerTokens.end(), Type);
                  if (iTypes != ClientCodes::MarkerTokens.end())
                      iMarker->operator[](iTypes - ClientCodes::MarkerTokens.begin()) = iInfo - info.begin();
              }
              else if (iBody != BodyChannels.end()) {
                  // The channel is for a body we already have.
                  iTypes = std::find(ClientCodes::BodyTokens.begin(), ClientCodes::BodyTokens.end(), Type);
                  if (iTypes != ClientCodes::BodyTokens.end())
                      iBody->operator[](iTypes - ClientCodes::BodyTokens.begin()) = iInfo - info.begin();
              } else if ((iTypes = std::find(ClientCodes::MarkerTokens.begin(), ClientCodes::MarkerTokens.end(), Type)) != ClientCodes::MarkerTokens.end()) {
                  // Its a new marker.
                  MarkerChannels.push_back(MarkerChannel(Name));
                  MarkerChannels.back()[iTypes - ClientCodes::MarkerTokens.begin()] = iInfo - info.begin();
              } else if ((iTypes = std::find(ClientCodes::BodyTokens.begin(), ClientCodes::BodyTokens.end(), Type)) != ClientCodes::BodyTokens.end()) {
                  // Its a new body.
                  BodyChannels.push_back(BodyChannel(Name));
                  BodyChannels.back()[iTypes - ClientCodes::BodyTokens.begin()] = iInfo - info.begin();
              } else if (Type == "<F>") {
                  //FrameChannel = iInfo - info.begin();
              } else {
                  // It could be a new channel type.
              }


          }

          ros::init(argc, argv, "EpuckViconPublisher");
          ros::NodeHandle n;


          std::vector<ros::Publisher> EpuckRobotsVector;
          int maxNumOfRobots = 40;

          EpuckRobotsVector.resize(maxNumOfRobots);
          std::stringstream TopicName;

          numOfRobots = OH_getNumOfRobots();

          //-- the below array is used further down when satisfying resource requests
          std::vector<int> prevRobotPortionIds(maxNumOfRobots,-1);
          std::vector<double> robotResourceAmounts(maxNumOfRobots,0);
          std::vector<Vector2D> robotPositions(maxNumOfRobots, Vector2D());
          std::vector<double> robotResourceLeftAmounts(maxNumOfRobots,0);
          std::vector<int> robotPortionIds(maxNumOfRobots,-1);


          int robotVectorIndex;



          //-- setup the topic that will tie this to the EpuckPositionSuscriber
          for(std::vector<ros::Publisher>::iterator RobotIterator = EpuckRobotsVector.begin(); RobotIterator!=EpuckRobotsVector.end(); RobotIterator++){
        	  robotVectorIndex = distance(EpuckRobotsVector.begin(),RobotIterator);

        	  if (robotVectorIndex < numOfRobots) {
				  int robotId = OH_ROBOT_IDS[robotVectorIndex];
				  TopicName << "/epuck" << robotId;
				  TopicName << "/Position";


				  EpuckRobotsVector[robotVectorIndex] = n.advertise<cpp_observer::ViconPosition>(TopicName.str().c_str(), 1000);

				  TopicName.str("");

				  std::cout << " Setting up robot " << robotId << " id in robots vector " << robotVectorIndex << std::endl;
        	  }
          }

          ros::Rate loop_rate(100);
          cpp_observer::ViconPosition Msg;


          //- Get Data - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
          // Get the data using the request/reply protocol.

          //int i;

          std::vector< double > data;
          data.resize(info.size());



          std::vector< MarkerData > markerPositions;
          markerPositions.resize(MarkerChannels.size());

          std::vector< BodyData > bodyPositions;
          bodyPositions.resize(BodyChannels.size());


          int resourceRequestRobotId, resourceRequestWorksiteId, resourcePortionId;
          double resourceRequestAmount;
          bool isReportDuplicate;

          timeLost += int(difftime(time(0), connectionErrorStartTime));
          std::cout << " connection error time lost " << timeLost << std::endl;

          while (user_quit != 1 && ros::ok()) {
        	  //-- restart the retial counter
        	  retrial_count = 0;

        	  //-- get current time
        	  timeSinceStart = int(difftime(time(0), startTime)) - timeLost;
        	  //std::cout << "." << timeSinceStart << std::endl;
        	  //double timestamp = data[FrameChannel];
        	  //std::cout<< timeSinceStart << std::endl;

        	  int *reportData = Server.GetInfoEventReportArray();
        	  if(reportData[0]!=0) {
        		  InfoEventReport report;
        		  report.robotId = reportData[0];
        		  report.globalTime = reportData[1];
        		  report.type = static_cast<InfoEventReport::TYPE>(reportData[2]);
        		  report.worksiteId = reportData[3];
        		  report.recruiterId = reportData[4];
        		  if (DEBUG_REPORT_RECEIVING == 1) { std::cout << " ~~~~~~ " << report.type << " (sent as " << reportData[2] << ")  t=" << report.globalTime << "  w id=" << reportData[3] << " rec id=" <<  reportData[4] << std::endl; }

        		  //-- sometimes, other data is ok but type is not
        		  if (report.globalTime > 0 && report.type == InfoEventReport::TYPE_VOID) {
        			  report.type = InfoEventReport::TYPE_UNKNOWN;
        			  std::cout << " ~~~~~~~~~~~ @!!!!!!" << std::endl;
        		  }
        		  //--
        		  if (report.type != InfoEventReport::TYPE_VOID) {

        			  //-- check if this is a duplicate
        			  isReportDuplicate = false;
        			  for (uint r=0; r<infoEventReports.size(); r++) {
        				  if (infoEventReports[r].GetIsTheSameAs(report)) {
        					  isReportDuplicate = true;
        					  break;
        				  }
        			  }

        			  //-- actual new report
        			  if (!isReportDuplicate) {
        				  infoEventReports.push_back(report);

        				  if (report.type == InfoEventReport::TYPE_REWARD_RECEIVED) {
        					  //-- special type of report that tells the server robot has received reward that it previously loaded. Only applies to foraging
        					  if (missionType == 1) {
        					  //std::cout << " REWARD RECEIVED " << (report.recruiterId / 10000.0) << std::endl;
        						  totalReward += report.recruiterId / 10000.0; // the recruiterId place was borrowed to report the amount
        					  }
        				  }

        				  outputFileInfoEvents << report.globalTime << "\t" << report.GetTypeDescriptionString() << "\t" << report.robotId << "\t"
        				  				  			<< report.worksiteId << "\t" << -1 << "\t" << -1 << "\t" << -1 << "\t" << report.recruiterId
													<< "\t" << timeSinceStart << std::endl;

        				  std::cout << " ~~~~~~~~~ INFO EVENT REPORT t=" << report.globalTime << " robot id=" << report.robotId << " " << report.GetTypeDescriptionString() << " worksite id=" << report.worksiteId << " recruiter id="<< report.recruiterId << std::endl;
        			  }
        		  }
				       		        		  //loop_rate.sleep();
        	  }

        	  //-- handle resource requests, from 1 robot at a time
        	  resourceRequestRobotId = -1;
        	  resourceRequestAmount = 0;
        	  resourceRequestWorksiteId = 0;
        	  resourcePortionId = -1;
        	  int *requestData = Server.GetResourceRequestArray();

        	  if(requestData[0]!=0) {

        		  resourceRequestRobotId = requestData[0];
        		  resourceRequestWorksiteId = requestData[1] / 1000;
        		  resourcePortionId = requestData[1] % 1000;
        		  resourceRequestAmount = requestData[2] / 10000.0;

        		  WorksiteData* worksite = GetWorksiteById(resourceRequestWorksiteId);
        		  if (worksite != NULL) {
					  if (DEBUG_WORKSITE_PROCESSING == 1) { std::cout << " <<< request  from robot " << resourceRequestRobotId << "portion id  " << resourcePortionId << " prev for 30 " << prevRobotPortionIds[30] << " prev for 38 " << prevRobotPortionIds[38] <<  " Current volume in W " << resourceRequestWorksiteId << " = " << worksite->volume << " requested " << resourceRequestAmount << std::endl; }

					  //-- extract from worksite
					  if (resourcePortionId == prevRobotPortionIds[resourceRequestRobotId]) {
						  //-- robot still waiting for the request to be satisfied
						  // std::cout << " robot " << resourceRequestRobotId << " trying to get " << resourceRequestAmount << std::endl;
						  robotPortionIds[resourceRequestRobotId] = resourcePortionId;
					  } else  {
						  //-- robot initiated new request
						  double robotDFromWorksite = sqrt(pow(robotPositions[resourceRequestRobotId].x - worksite->xPos,2) + pow(robotPositions[resourceRequestRobotId].y - worksite->yPos,2));
						  if (DEBUG_WORKSITE_PROCESSING == 1) { std::cout << " robot " << resourceRequestRobotId << " new request: robot pos= " << robotPositions[resourceRequestRobotId].GetOutputString() << " worksite pos=[" << worksite->xPos << ";" << worksite->yPos << "]  D=" << robotDFromWorksite; }

						  if (worksite->isActive && robotDFromWorksite*1.1 <= worksiteRadius) { // make sure worksite has volume as robot is actually on it (with some margin of error)
							  //-- alter the amount the robot will get if worksite has less volume than requested
							  if (worksite->volume < resourceRequestAmount) {
								robotResourceAmounts[resourceRequestRobotId] = worksite->volume;
							  } else {
								robotResourceAmounts[resourceRequestRobotId] =resourceRequestAmount;
							  }
							  robotPortionIds[resourceRequestRobotId] = resourcePortionId;;

							  //-- extract worksite volume
							  worksite->volume -= robotResourceAmounts[resourceRequestRobotId];
							  //-- give it to robot
							  robotResourceLeftAmounts[resourceRequestRobotId] = worksite->volume;

							  //-- if this is task allocation, immediatelly increase reward (but foraging relies on robot's reports of getting reward)
							  if (missionType == 0) {
								  totalReward += robotResourceAmounts[resourceRequestRobotId];
							  }

							  if (DEBUG_WORKSITE_PROCESSING == 1) { std::cout << "will get " << robotResourceAmounts[resourceRequestRobotId] << " NEW WORKSITE VOLUME " << worksite->volume << std::endl; }
							  if (DEBUG_WORKSITE_VOLUME_LEFT == 1) { std::cout << " worksite " << worksite->id << " vol left " << worksite->volume << std::endl; }
							  if (worksite->volume < MIN_REWARD_NO) {
								  worksite->isActive = false;
								  if (DEBUG_WORKSITE_PROCESSING == 1 || true) { std::cout << "!!!!!! DEPLETED WORKSITE " << worksite->id << std::endl;}

								  outputFileInfoEvents << timeSinceStart << "\t" << EVENT_WORKSITE_COMPLETED << "\t" << -1 << "\t"
								  				  	  	 << worksite->id << "\t" << worksite->xPos << "\t" << worksite->yPos << "\t"
								  						 << worksite->volume << "\t" << -1 << "\t" << timeSinceStart << std::endl;
							  }
						  } else {
							  //-- request not valid - no more resource in the worksite or robot is somewhere else
							  if (DEBUG_WORKSITE_PROCESSING == 1) { std::cout << " worksite not valid " << std::endl; }
							  robotResourceAmounts[resourceRequestRobotId] = 0;
							  robotResourceLeftAmounts[resourceRequestRobotId] = 0;
							  robotPortionIds[resourceRequestRobotId] = resourcePortionId;

						  }


					  }

					  prevRobotPortionIds[resourceRequestRobotId] = resourcePortionId;
        		  }



        		  //loop_rate.sleep();
        	  }

        	  int *robotId = Server.GetCurrentSenderID();
        	  if(*robotId!=0) {
        		  ros::spinOnce();
        	  }
        	  //std::cout << "----";


              pBuff = buff;

              * ((int32_t *) pBuff) = ClientCodes::EData;
              pBuff += sizeof(int32_t);
              * ((int32_t *) pBuff) = ClientCodes::ERequest;
              pBuff += sizeof(int32_t);

              if (send(sockfd, buff, pBuff - buff, 0) == SOCKET_ERROR)
                  throw std::string("Error Requesting");

              int32_t packet;
              int32_t type;

              // Get and check the packet header.

              if (!recieve(sockfd, packet))
                  throw std::string("Error Recieving");

              if (!recieve(sockfd, type))
                  throw std::string("Error Recieving");

              if (type != ClientCodes::EReply)
                  throw std::string("Bad Packet");

              if (packet != ClientCodes::EData)
                  throw std::string("Bad Reply Type");

              if (!recieve(sockfd, size))
                  throw std::string();

              if (size != info.size())
                  throw std::string("Bad Data Packet");

              // Get the data.

              std::vector< double >::iterator iData;

              for (iData = data.begin(); iData != data.end(); iData++) {
                  if (!recieve(sockfd, *iData))
                      throw std::string();
              }


              //- Look Up Channels - - - - - - - - - - - - - - - - - - - - - - -

              // Get the channels corresponding to the markers.
              // Y is up
              // The values are in millimeters

              std::vector< MarkerChannel >::iterator iMarker;
              std::vector< MarkerData >::iterator iMarkerData;

              for ( iMarker = MarkerChannels.begin(), iMarkerData = markerPositions.begin(); iMarker != MarkerChannels.end(); iMarker++, iMarkerData++) {
                  iMarkerData->X = data[iMarker->X];
                  iMarkerData->Y = data[iMarker->Y];
                  iMarkerData->Y = data[iMarker->Z];
                  if (data[iMarker->O] > 0.5)
                      iMarkerData->Visible = false;
                  else
                      iMarkerData->Visible = true;

                  // std::cout << iMarker->Name << ":(" << iMarkerData->X << "," << iMarkerData->Y << "," << iMarkerData->Z << ")" << std::endl;
              }

              // Get the channels corresponding to the bodies.
              //=================================================================
              // The bodies are in global space
              // The world is Z-up
              // The translational values are in millimeters
              // The rotational values are in radians
              //=================================================================

              std::vector< BodyChannel >::iterator iBody;
              std::vector< BodyData >::iterator iBodyData;
              std::vector<int> sensedWorksiteIds;

              int numOfRobotsDetected = 0;

              for ( iBody = BodyChannels.begin(), iBodyData = bodyPositions.begin(); iBody != BodyChannels.end(); iBody++, iBodyData++) {

            	  robotData = ProcessBodyData(iBodyData, iBody, data);
            	  robotVectorIndex = OH_getRobotVectorIndex(robotData.id);
            	  int robotId = robotData.id;
            	  numOfRobotsDetected++;
            	  if (robotVectorIndex >= 0) {
					  double rotDegrees = (robotData.pa / 10000.0) * (180/PI);

					  std::stringstream EpuckName;
					  EpuckName << "epuck" << robotData.id;
					  Msg.Name = EpuckName.str();
					  Msg.globalTime.data = timeSinceStart; //robotData.id;//
					  Msg.X.data = robotData.px;
					  Msg.Y.data = robotData.py;
					  Msg.A.data = robotData.pa;

					  robotPositions[robotId].x = int(robotData.px);
					  robotPositions[robotId].y = robotData.py;

					  if (DEBUG_ROBOT_MESSAGES) { std::cout << "robot " << robotData.id <<  " (index " << robotVectorIndex << ") X: " << robotData.px << " Y: " << robotData.py << " R: " << rotDegrees ; }

					  /*for (int i=30; i<39; i++) {
						  std::cout << i << ":" << robotPositions[i].x << "  ";
					  }
					  std::cout << std::endl;*/
					  //robotPositions[30]->x = int(robotData.px);

					  //-- find out if the robot is close to a worksite. if close to multipe worksites, pick a random one
					  Msg.worksite1Id.data = 0; // first set sensed worksite id to invalid id
					  Msg.worksite1X.data = 0;
					  Msg.worksite1Y.data = 0;
					  sensedWorksiteIds.clear();

					  for (uint i=0; i<worksites.size(); i++) {
						  //std::cout << " W" << worksites[i].id << " index " << i ;
						  if (worksites[i].isActive) {
							  //std::cout << "active";
							  if (GetCanRobotSenseWorksite(robotData.px, robotData.py, worksites[i])) {
								  sensedWorksiteIds.push_back(i);
								  //std::cout << "sensing";
							  }
						  }
						  //std::cout << std::endl;
					  }
					  if (sensedWorksiteIds.size() > 0) {
						  if (DEBUG_ROBOT_MESSAGES) { std::cout << " ! SENSING " << sensedWorksiteIds.size() << " worksites"; }
						  int pickedWorksiteSensedArrayId = std::rand() % sensedWorksiteIds.size();
						  int pickedWorksiteArrayId = sensedWorksiteIds[pickedWorksiteSensedArrayId];
						  //std::cout << " robot " << robotData.id << "  picked " <<  pickedWorksiteArrayId << " out of " << sensedWorksiteIds.size() << std::endl;
						  Msg.worksite1Id.data = worksites[pickedWorksiteArrayId].id;
						  Msg.worksite1X.data = worksites[pickedWorksiteArrayId].xPos;
						  Msg.worksite1Y.data = worksites[pickedWorksiteArrayId].yPos;

						  if (DEBUG_ROBOT_MESSAGES) { std::cout << " > W" << worksites[pickedWorksiteArrayId].id << "(" << worksites[pickedWorksiteArrayId].xPos << ";" << worksites[pickedWorksiteArrayId].yPos << ")"; }

					  }


					  //-- give the robot resource if requested
					  if (robotResourceAmounts[robotData.id] > 0.01 && robotPortionIds[robotData.id] > 0) {
						  if (DEBUG_ROBOT_MESSAGES) { std::cout << "  ! GETTING RES " << robotResourceAmounts[robotData.id] << " from W" << resourceRequestWorksiteId << " portionId=" << robotPortionIds[robotData.id]; }
					  }
					  Msg.resourceReceived.data = robotResourceAmounts[robotData.id] * 10000;
					  Msg.resourceLeft.data = robotResourceLeftAmounts[robotData.id] * 10000;
					  Msg.resourcePortionId.data = robotPortionIds[robotData.id];

					  if (DEBUG_ROBOT_MESSAGES) { std::cout << std::endl; }

					  EpuckRobotsVector[robotVectorIndex].publish(Msg);
					  //std::cout << "t=" << timeSinceStart << " robot " << robotData.id << "  num subscribers " << EpuckRobotsVector[robotVectorIndex].getNumSubscribers() << std::endl;
            	  } else {
            		  std::cout << " !!! INVALID ROBOT VECTOR INDEX FOR robot " << robotId << std::endl;
            	  }

                  ros::spinOnce();
                  loop_rate.sleep();			
              }

              //std::cout << "t=" << timeSinceStart << " num of robots detected " << numOfRobotsDetected << std::endl;


              //-- output to global file
              /*if (timeSinceStart != lastReportedGlobalTimeSinceStart) {
            	  outputFileGlobal << timeSinceStart << "\t" << totalReward << std::endl;
            	  lastReportedGlobalTimeSinceStart = timeSinceStart;
              }*/



              usleep(refreshDelay);

          }
      }

      catch (const std::string & rMsg) {
          if (rMsg.empty())
              std::cout << "Error! Error! Error! Error! Error!" << std::endl;
          else
              std::cout << " !!!!!!!!!!!!!!!!!!!! " << rMsg.c_str() << std::endl;
      }

      if (retrial_count == 0) {
    	  connectionErrorStartTime = time(0);
    	  std::cout << " connection error started at " << connectionErrorStartTime << std::endl;
      }

      if (retrial_count < 100) {
    	  //restart the request if error occurs
    	  std::cout << " SLEEPING " << std::endl;
      	  usleep(refreshDelay*3);
      } else {
    	  std::cout << " TRYING TO RECONNECT TO VICON " << std::endl;

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


    	  if ((he = gethostbyname(HOST)) == NULL) {
    		  std::cout << " get host by name error" << std::endl;
    		  usleep(refreshDelay*10);
    		  continue;
    	  }

    	  if ((sockfd = socket(PF_INET, SOCK_STREAM, 0)) == -1) {
    		  std::cout << " socket error" << std::endl;
    		  usleep(refreshDelay*10);
    		  continue;
    	  }

    	  their_addr.sin_family = AF_INET;    // host byte order
    	  their_addr.sin_port = htons(PORT);    // short, network byte order
    	  their_addr.sin_addr = *((struct in_addr *)he->h_addr);
    	  memset(their_addr.sin_zero, '\0', sizeof their_addr.sin_zero);

    	  if (connect(sockfd, (struct sockaddr *)&their_addr, sizeof their_addr) == -1) {
    		  std::cout << " !!! ERROR connecting" << std::endl;
    		  usleep(refreshDelay*10);
    	  }
      }
  }

  std::cout << " EXITING " << std::endl;
  //--close files
  //outputFileGlobal.close();
  outputFileInfoEvents.close();

  //--close connection
  close(sockfd);
  //close(gps_sockfd);

  return 0;
}

/* ============================== I/O ===================*/
void ProcessParameterFile() {
	std::cout << "---------------- READING 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_NUM_OF_WORKSITES) {
				numOfWorksites = paramVal;
				std::cout << PARAM_NUM_OF_WORKSITES << " = " << numOfWorksites << std::endl;
			} else if (paramName == PARAM_MIN_WORKSITE_D) {
				minWorksiteD = paramVal;
				std::cout << PARAM_MIN_WORKSITE_D << " = " << minWorksiteD << std::endl;
			} else if (paramName == PARAM_MAX_WORKSITE_D) {
				maxWorksiteD = paramVal;
				std::cout << PARAM_MAX_WORKSITE_D << " = " << maxWorksiteD << std::endl;
			} else if (paramName == PARAM_WORKSITE_RADIUS) {
				worksiteRadius = paramVal;
				std::cout << PARAM_WORKSITE_RADIUS << " = " << worksiteRadius << std::endl;
			} else if (paramName == PARAM_WORKSITE_VOLUME) {
				worksiteVolume = paramVal;
				std::cout << PARAM_WORKSITE_VOLUME << " = " << worksiteVolume << std::endl;
			} else if (paramName == PARAM_ROBOT_WORKSITE_SENSE_RANGE) {
				robotWorksiteSenseRange = paramVal;
				std::cout << PARAM_ROBOT_WORKSITE_SENSE_RANGE << " = " << robotWorksiteSenseRange << std::endl;
			} else if (paramName == PARAM_BASE_X) {
				baseX = paramVal;
				std::cout << PARAM_BASE_X << " = " << baseX << std::endl;
			} else if (paramName == PARAM_BASE_Y) {
				baseY = paramVal;
				std::cout << PARAM_BASE_Y << " = " << baseY << std::endl;
			} else if (paramName == PARAM_BASE_RADIUS) {
				baseRadius = paramVal;
				std::cout << PARAM_BASE_RADIUS << " = " << baseRadius << std::endl;
			} else if (paramName == PARAM_MISSION_TYPE) {
				missionType = paramVal;
				std::cout << PARAM_MISSION_TYPE << " = " << missionType << std::endl;
			}
		}
	} else {
		std::cout << "!!!! Unable to open file" << std::endl;
	}
	file.close();
	std::cout << std::endl;
	OH_processParameterFile();
	std::cout << "----------------------------------------------------" << std::endl;


}

/* ============================== WORLD EDITING ===================*/
void GenerateWorld() {

	worksites.clear();




	int minX = -9100;
	int maxX = 9650;
	int minY = -6600;
	int maxY = 7470;

	int minDBetweenWorksites = worksiteRadius*2.5;
	if (numOfWorksites < 10) {
		minDBetweenWorksites = worksiteRadius*5;
	}

	//-- make sure heap1 environments don't have the worksite very near border
	if (numOfWorksites < 3) {
		int arenaxMin=-9370;
		int arenayMin=-6800;
		int arenaxMax=9850;
		int arenayMax=7760;

		int robotSize = 750; //-- robot has 7.5cm in diameter, which is 750 units
		int heapFreeSpace = 2*robotSize;
		minX = arenaxMin + heapFreeSpace;
		maxX = arenaxMax - heapFreeSpace;
		minY = arenayMin + heapFreeSpace;
		maxY = arenayMax - heapFreeSpace;
	}

	int xRange = maxX - minX;
	int yRange = maxY - minY;
	int maxNumOfTriesLeft = 300;

	double dToBase, dToWorksite;
	Vector2D* generatedPos;

	for (int i=0; i<numOfWorksites; i++) {
		bool positionOk = false;
		if (DEBUG_WORLD_GENERATION == 1) { std::cout << " placing worksite " << i << std::endl; }

		while (!positionOk) {
			generatedPos = new Vector2D(minX + rand()%xRange, minY + rand()%yRange);
			dToBase = sqrt(pow(generatedPos->x - baseX,2) + pow(generatedPos->y - baseY,2));
			if (DEBUG_WORLD_GENERATION == 1) { std::cout << " generated " << generatedPos->GetOutputString() << " d to base " << dToBase; }
			if (dToBase >= minWorksiteD && dToBase <= maxWorksiteD) {
				//-- ok distance from base, now check distance from the rest of worksites
				bool isAWorksiteTooClose = false;
				for (int w=0; w<i; w++) {
					dToWorksite = sqrt(pow(generatedPos->x - worksites[w].xPos,2) + pow(generatedPos->y - worksites[w].yPos,2));
					if (DEBUG_WORLD_GENERATION == 1) { std::cout << " d to worksite" << w << " " << dToWorksite; }
					if (dToWorksite < minDBetweenWorksites) {
						isAWorksiteTooClose = true;
						break;
					}
				}
				if (!isAWorksiteTooClose) {
					if (DEBUG_WORLD_GENERATION == 1) { std::cout << " !!!OK!!!"; }
					positionOk = true;
				}
			}
			if (DEBUG_WORLD_GENERATION == 1) { std::cout << std::endl;}
		}


		if (positionOk) {
			AddWorksite(i+1, generatedPos);
		} else {
			maxNumOfTriesLeft--;
			if (maxNumOfTriesLeft <= 0) {
				std::cout << " !!! REACHED MAX NUM OF TRIES, FAILED TO GENERATE WORLD " << std::endl;
				exit(0);
			}
		}
	}

	//-- pre-set world 3 worksites
	/*worksites.clear();
	worksiteVolume = 30;
	//AddWorksite(1, new Vector2D(3460,-2645));
	//AddWorksite(2, new Vector2D(-2838,4140));
	AddWorksite(1, new Vector2D(-1593,3055));*/

}

void AddWorksite(int id_, Vector2D* position_) {
	std::cout << " Adding worksite " << id_ << " at " << position_->GetOutputString() << " volume " << worksiteVolume <<  std::endl;
	worksites.push_back(WorksiteData(id_,position_->x,position_->y,worksiteVolume));
	int index = worksites.size()-1;
	outputFileInfoEvents << timeSinceStart << "\t" << EVENT_WORKSITE_ADDED << "\t" << -1 << "\t"
				  	  	 << worksites[index].id << "\t" << worksites[index].xPos << "\t" << worksites[index].yPos << "\t"
						 << worksites[index].volume << "\t" << -1 << "\t" << timeSinceStart << std::endl;
}

WorksiteData* GetWorksiteById(int id_) {
	for (uint i=0; i<worksites.size(); i++) {
		if (worksites[i].id == id_) {
			return &worksites[i];
		}
	}
	return NULL;
}

/* ============================== REMOTE SENSING FUNCTIONS ===================*/

bool GetCanRobotSenseWorksite(int robotX_, int robotY_, WorksiteData worksiteData_) {

	double distance = sqrt( pow(worksiteData_.xPos - robotX_,2) + pow(worksiteData_.yPos - robotY_,2));
	return distance <= robotWorksiteSenseRange;
}



robot_position_t ProcessBodyData(std::vector<BodyData>::iterator iBodyData, std::vector<BodyChannel>::iterator iBody, std::vector< double > data) {
	iBodyData->TX = data[iBody->TX];
	iBodyData->TY = data[iBody->TY];
	iBodyData->TZ = data[iBody->TZ];

	// The channel data is in the angle-axis form.
	// The following converts this to a quaternion.
	//=============================================================
	// An angle-axis is vector, the direction of which is the axis
	// of rotation and the length of which is the amount of
	// rotation in radians.
	//=============================================================

	double len, tmp;
	robot_position_t pos_data;

	len = sqrt( data[iBody->RX] * data[iBody->RX] +
		  data[iBody->RY] * data[iBody->RY] +
		  data[iBody->RZ] * data[iBody->RZ]);

	iBodyData->QW = cos(len / 2.0);
	tmp = sin(len / 2.0);
	if (len < 1e-10)
	{
	  iBodyData->QX = data[iBody->RX];
	  iBodyData->QY = data[iBody->RY];
	  iBodyData->QZ = data[iBody->RZ];
	}
	else
	{
	  iBodyData->QX = data[iBody->RX] * tmp / len;
	  iBodyData->QY = data[iBody->RY] * tmp / len;
	  iBodyData->QZ = data[iBody->RZ] * tmp / len;
	}

	// The following converts angle-axis to a rotation matrix.

	double c, s, x, y, z;
	y = 0;
	if (len < 1e-15) {
	  iBodyData->GlobalRotation[0][0] = iBodyData->GlobalRotation[1][1] = iBodyData->GlobalRotation[2][2] = 1.0;
	  iBodyData->GlobalRotation[0][1] = iBodyData->GlobalRotation[0][2] = iBodyData->GlobalRotation[1][0] =
		  iBodyData->GlobalRotation[1][2] = iBodyData->GlobalRotation[2][0] = iBodyData->GlobalRotation[2][1] = 0.0;
	} else {
	  x = data[iBody->RX] / len;
	  y = data[iBody->RY] / len;
	  z = data[iBody->RZ] / len;

	  c = cos(len);
	  s = sin(len);

	  iBodyData->GlobalRotation[0][0] = c + (1 - c) * x * x;
	  iBodyData->GlobalRotation[0][1] =     (1 - c) * x * y + s * (-z);
	  iBodyData->GlobalRotation[0][2] =     (1 - c) * x * z + s * y;
	  iBodyData->GlobalRotation[1][0] =     (1 - c) * y * x + s * z;
	  iBodyData->GlobalRotation[1][1] = c + (1 - c) * y * y;
	  iBodyData->GlobalRotation[1][2] =     (1 - c) * y * z + s * (-x);
	  iBodyData->GlobalRotation[2][0] =     (1 - c) * z * x + s * (-y);
	  iBodyData->GlobalRotation[2][1] =     (1 - c) * z * y + s * x;
	  iBodyData->GlobalRotation[2][2] = c + (1 - c) * z * z;
	}

	// now convert rotation matrix to nasty Euler angles (yuk)
	// you could convert direct from angle-axis to Euler if you wish

	// 'Look out for angle-flips, Paul...'
	//  Algorithm: GraphicsGems II - Matrix Techniques VII.1 p 320
	assert(fabs(iBodyData->GlobalRotation[0][2]) <= 1);
	iBodyData->EulerY = asin(-iBodyData->GlobalRotation[2][0]);

	if (fabs(cos(y)) > std::numeric_limits<double>::epsilon() ) {
	  iBodyData->EulerX = atan2(iBodyData->GlobalRotation[2][1], iBodyData->GlobalRotation[2][2]);
	  iBodyData->EulerZ = atan2(iBodyData->GlobalRotation[1][0], iBodyData->GlobalRotation[0][0]);
	} else {
	  iBodyData->EulerZ = 0;
	  iBodyData->EulerX = atan2(iBodyData->GlobalRotation[0][1], iBodyData->GlobalRotation[1][1]);
	}

	// std::cout << iBody->Name << ":(" << iBody->RX << "," << iBody->RY << "," << iBody->RZ << ")" << std::endl;

	// extract the robot id number.
	std::string id_num;
	char *result;
	char buf[30];
	unsigned int i;

	memset(buf, '\0', 30);
	//std::cout << iBody->Name << std::endl;
	iBody->Name.copy(buf, 24);
	result = strtok( buf, ":");
	pos_data.id = 0;
	while( result != NULL ) {
	  // loop through string checking for digit
	  for( i = 0; i < strlen(result); i++ ) {
		  if( isdigit(result[i]) ) {
			  id_num += result[i];
		  }
	  }
	  result = strtok( NULL, ":");

	  pos_data.id = atoi(id_num.c_str());
	  //printf("ID: %d\n", pos_data.id);
	  id_num = "";
	}

	// fill in the rest of the struct
	pos_data.px = (int)(10 * iBodyData->TX);
	pos_data.py = (int)(10 * iBodyData->TY);
	pos_data.pa = -(int)(10000 * iBodyData->EulerZ); // has to  be sent in multiplies of 10000 because only int possible

	return pos_data;
}


/* =================================== LOW LEVEL STUFF =====================*/

bool recieve(int Socket, char * pBuffer, int BufferSize) {
	char * p = pBuffer;
	char * e = pBuffer + BufferSize;
	int result;
	while (p != e) {
		result = recv( Socket, p, e - p, 0 );

		if (result == -1)
			return false;

		p += result;
	}
	return true;
}

bool recieve(int Socket, int32_t & Val) { return recieve(Socket, (char*) & Val, sizeof(Val)); }
bool recieve(int Socket, uint32_t & Val) { return recieve(Socket, (char*) & Val, sizeof(Val));  }
bool recieve(int Socket, double & Val) { return recieve(Socket, (char*) & Val, sizeof(Val)); }

void signalHandler(int dummy) {
    printf("Captured ctrl+c\n");
    user_quit = 1;
    exit(0);
}
