#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"
#include "../../cpp_common/mathHelpers.h"

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

#define DEBUG_MESSAGE_RECEIVING 1
#define DEBUG_ROBOT_POSITIONS 1
#define DEBUG_MESSAGE_SENDING 1

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


//-- 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();
double GetDistanceBetweenVectors(Vector2D pos1, Vector2D pos2);
bool GetCanRobotSenseCommunication(Vector2D senderPos, Vector2D robotPos);

//-- global vars
int user_quit;

int numOfRobots;
int commMaxD;

struct RecruitmentMessage {
	int worksiteId;
	Vector2D worksitePos;
	int recruiterId;

	RecruitmentMessage() {
		worksiteId = -1;
		worksitePos = Vector2D();
		recruiterId = 0;
	}
};

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

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

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

  int refreshDelay = 0;

  //-- init time
  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;
  }

  //-- setup parameters and other stuff
  ProcessParameterFile();

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

  //-- other stuff
  robot_position_t robotData; // transmission packet containing dats
  int sockfd;
  struct hostent *he;
  struct sockaddr_in their_addr;

  // 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, "EpuckCommPublisher");
          ros::NodeHandle n;


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

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

          numOfRobots = OH_getNumOfRobots();

          std::vector<Vector2D> robotPositions(maxNumOfRobots, Vector2D());
          std::vector<RecruitmentMessage> recruitmentMessages (maxNumOfRobots, RecruitmentMessage());

          int robotVectorIndex;

          //-- setup the topic that will tie this to the EpuckCommSuscriber
          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 << "/Comm";


				  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 worksiteX, worksiteY, worksiteId;
          std::vector<int> possibleReceieverIds;

          uint senderId, id;
          double distanceBetweenRobots, randNum;

          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;

        	  //----------- get robot sent messages
        	  int *commData = Server.GetRobotCommDataArray();
        	  senderId = commData[0];
        	  worksiteId = commData[1];
        	  worksiteX = commData[2];
        	  worksiteY = commData[3];


        	  //std::cout << senderId << std::endl;
        	  if(senderId > 0 && senderId < 40 && worksiteId > 0 ) {
        		  if (DEBUG_MESSAGE_RECEIVING == 1) { std::cout << commData[0] << " sent: " << worksiteX << ";" << worksiteY << std::endl; }
        		  //-- find all robots that can get the message
        		  possibleReceieverIds.clear();
        		  for (id = 0; id< robotPositions.size(); id++) {
        			  if (senderId != id  && !(robotPositions[id].x == 0 && robotPositions[id].y == 0)) {
        				  if (DEBUG_ROBOT_POSITIONS == 1) { std::cout << " msg from robot " << senderId << " " << robotPositions[senderId].GetOutputString() << " checking for robot " << id << " pos " << robotPositions[id].GetOutputString(); }

        				  if (GetCanRobotSenseCommunication(robotPositions[senderId],robotPositions[id])) {
        					  if (DEBUG_ROBOT_POSITIONS == 1) { std:: cout << " CAN SENSE"; }
        					  possibleReceieverIds.push_back(id);
        				  }

        				  if (DEBUG_ROBOT_POSITIONS == 1) { std::cout << std::endl; }

        			  }
        		  }

        		  //-- pick one robot that will get the message. Send the message with gaussian probability based on distance to sender
        		  if (possibleReceieverIds.size() > 0) {
					  int pickedPossibleReceiversArrayId = std::rand() % possibleReceieverIds.size();
					  int pickedReceiverId = possibleReceieverIds[pickedPossibleReceiversArrayId];
					  distanceBetweenRobots = GetDistanceBetweenVectors(robotPositions[senderId], robotPositions[pickedReceiverId]);
					  randNum = fabs(mathHelpers_generateRandGauss(commMaxD/2.0));

					  if (DEBUG_ROBOT_POSITIONS) { std::cout << " send to robot " << pickedReceiverId << " randN=" << randNum << " d=" <<distanceBetweenRobots; }

					  if (distanceBetweenRobots <= randNum) {

						  recruitmentMessages[pickedReceiverId].worksiteId = worksiteId;
						  recruitmentMessages[pickedReceiverId].worksitePos.x = worksiteX;
						  recruitmentMessages[pickedReceiverId].worksitePos.y = worksiteY;
						  recruitmentMessages[pickedReceiverId].recruiterId = senderId;

						  //-- use special message to the sender to tell it that it will be exchanging data
						  //recruitmentMessages[senderId].worksiteId = RECRUITMENT_ACTIVE_DUMMY_NO;

					  	  if (DEBUG_ROBOT_POSITIONS) { std::cout << " - " << senderId << " SENDING to  " << pickedReceiverId; }
					  }
					  if (DEBUG_ROBOT_POSITIONS) { std::cout << std::endl;}

				  }

        	  }

        	  int *robotId = Server.GetCurrentSenderID();
        	  if(*robotId!=0) {
        		  ros::spinOnce();
        	  }

        	  //-----------

              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;

              for ( iBody = BodyChannels.begin(), iBodyData = bodyPositions.begin(); iBody != BodyChannels.end(); iBody++, iBodyData++) {
            	  //std::cout << iBody->Name << std::endl;
            	  robotData = ProcessBodyData(iBodyData, iBody, data);
            	  robotVectorIndex = OH_getRobotVectorIndex(robotData.id);
            	  int robotId = robotData.id;

            	  if (robotVectorIndex >= 0) {

            		  //double rotDegrees = (robotData.pa / 10000.0) * (180/PI);
					  //if (DEBUG_ROBOT_POSITIONS) { std::cout << "robot " << robotId <<  " (index " << robotVectorIndex << ") X: " << robotData.px << " Y: " << robotData.py << " R: " << rotDegrees ; }
					  robotPositions[robotId].x = int(robotData.px);
					  robotPositions[robotId].y = robotData.py;

					  //if (DEBUG_ROBOT_POSITIONS) { std::cout << std::endl; }

					  //-- send recruitment message to robot if it should receive one
					  Msg.worksite1Id.data = -1;
					  if ( !(recruitmentMessages[robotId].worksiteId == -1) ) {
						  if (DEBUG_MESSAGE_SENDING == 1) { std::cout << " sending w" << recruitmentMessages[robotId].worksiteId << " data to robot " << robotId << std::endl; }
						  Msg.worksite1Id.data = recruitmentMessages[robotId].worksiteId;
						  Msg.worksite1X.data = recruitmentMessages[robotId].worksitePos.x;
						  Msg.worksite1Y.data = recruitmentMessages[robotId].worksitePos.y;
						  Msg.resourceReceived.data = recruitmentMessages[robotId].recruiterId;

					  }
					  EpuckRobotsVector[robotVectorIndex].publish(Msg);
            	  } else {
            		  //std::cout << " !!! INVALID ROBOT VECTOR INDEX FOR robot " << robotId << std::endl;
            	  }


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


              }

              //-- clear recruitment messages
			  if (DEBUG_MESSAGE_SENDING == 1) {
				  //std::cout << "-- Clearing messages " << std::endl;
				  for (uint i=0; i<recruitmentMessages.size(); i++) {
					  recruitmentMessages[i] = RecruitmentMessage();
				  }
			  }
              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 < 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;
    	  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 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_COMM_MAX_D) {
				commMaxD = paramVal;
				std::cout << paramName << " = " << commMaxD << std::endl;
			}
		}
	} else {
		std::cout << "!!!! Unable to open file" << std::endl;
	}
	file.close();
	std::cout << std::endl;
	OH_processParameterFile();
	std::cout << "----------------------------------------------------" << std::endl;
}

/* ============================== REMOTE SENSING FUNCTIONS ===================*/
bool GetCanRobotSenseCommunication(Vector2D senderPos, Vector2D robotPos) {
	return GetDistanceBetweenVectors(senderPos,robotPos) <= commMaxD;
}

double GetDistanceBetweenVectors(Vector2D pos1, Vector2D pos2) {
	return sqrt( pow(pos1.x - pos2.x,2) + pow(pos1.y - pos2.y,2));
}



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);
}
