
#include <epuck.hh>
#include <EPuckBoardController.h>
#include <ethlolmsg.h>
#include <camera.h>
#include <vector>
#include <iostream>
#include <fstream>

#include <Vector2D.h>
#include "../../../cpp_common/constants.h"
#include "../../../cpp_common/mathHelpers.h"
#include <constants.h>
#include <IPCInterface.h>

using namespace std;

IPCInterface::IPCInterface(string hostIpAddress_, int hostPortNo_, Robot* robot_, EPuckBoardController* epuckBoardController_){
	this->hostIpAddress = hostIpAddress_;
	this->hostPortNo = hostPortNo_;

	this->robot = robot_;
	this->epuckBoardController = epuckBoardController_;

	timeTillNextPositionUpdate = 0;
}

void IPCInterface::Init(){
	cout << "Init IPC Client for ROS Process" << endl;
	ipc.SetCallback(ReceiveCallback, (void*)this);
	ipc.Start(hostIpAddress.c_str(), hostPortNo, false);
}

void IPCInterface::Update(uint64_t timeStamp_) {
	if (timeTillNextPositionUpdate > 0) {
		timeTillNextPositionUpdate--;
	}
	//std::cout << timeTillNextPositionUpdate << std::endl;
}

void IPCInterface::ProcessParameterFile(Robot* robot_) {
	std::cout << "---------------- READING PARAMETERS ----------------" << std::endl;

	std::ifstream file ("runSettings.txt");
	std::string line;

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

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

			linestream >> paramName >> paramValRaw;
			paramVal = std::atoi(paramValRaw.c_str());

			if (paramName == PARAM_MISSION_TYPE) {
				robot_->isDoingForaging = (paramVal == 1);
				std::cout << "foraging? " << " = " << robot_->isDoingForaging << std::endl;
			} else if (paramName == PARAM_WORKSITE_RADIUS) {
				robot_->worksiteRadius = paramVal;
				std::cout << paramName << " = " << robot_->worksiteRadius << std::endl;
			} else if (paramName == PARAM_BASE_X) {
				robot_->basePosition->x = paramVal;
				std::cout << paramName << " = " << robot_->basePosition->x << std::endl;
			} else if (paramName == PARAM_BASE_Y) {
				robot_->basePosition->y = paramVal;
				std::cout << paramName << " = " << robot_->basePosition->y << std::endl;
			} else if (paramName == PARAM_BASE_RADIUS) {
				robot_->baseRadius = paramVal;
				std::cout << paramName << " = " << robot_->baseRadius << std::endl;
			} else if (paramName == PARAM_TASK_ALLOCATION_LOAD_AMOUNT) {
				robot_->taskAllocationLoadAmount = std::atof(paramValRaw.c_str());
				std::cout << paramName << " = " << robot_->taskAllocationLoadAmount << std::endl;
			} else if (paramName == PARAM_FORAGING_LOAD_AMOUNT) {
				robot_->foragingLoadAmount = std::atof(paramValRaw.c_str());
				std::cout << paramName << " = " << robot_->foragingLoadAmount << std::endl;
			} else if (paramName == PARAM_CONTROLLER_TYPE) {
				robot_->controllerType = paramVal;
				std::cout << paramName << " = " << robot_->controllerType << std::endl;
			} else if (paramName == PARAM_TIME_BETWEEN_GLOBAL_POSITION_UPDATES) {
				robot_->timeBetweenGlobalPositionUpdates = paramVal;
				std::cout << paramName << " = " << robot_->timeBetweenGlobalPositionUpdates << std::endl;
			} else if (paramName == PARAM_GLOBAL_POSITION_ERROR_VARIANCE) {
				robot_->globalPositionErrorVariance = paramVal;
				std::cout << paramName << " = " << robot_->globalPositionErrorVariance << std::endl;
			} else if (paramName == PARAM_GLOBAL_ROTATION_ERROR_VARIANCE) {
				robot_->globalRotationErrorVariance = paramVal;
				std::cout << paramName << " = " << robot_->globalRotationErrorVariance << std::endl;
			} else if (paramName == PARAM_REPORT_SEND_TRIALS) {
				robot_->reportSendTrials = paramVal;
				std::cout << paramName << " = " << robot_->reportSendTrials << std::endl;
			} else if (paramName == PARAM_THIS_ROBOT_ID) {
				robot_->thisRobotId = paramVal;
				std::cout << paramName << " = " << robot_->thisRobotId << std::endl;
			} else if (paramName == PARAM_LOG_FILE_PREFIX) {
				robot_->logFilePrefix = paramValRaw;
				std::cout << paramName << " = " << robot_->logFilePrefix << std::endl;
			} else if (paramName == PARAM_COMM_TIME) {
				robot_->commTime = paramVal;
				std::cout << paramName << " = " << robot_->commTime << std::endl;
			} else if (paramName == PARAM_LB_NEAR_WORKSITE_D) {
				robot_->lbNearWorksiteD = paramVal;
				std::cout << paramName << " = " << robot_->lbNearWorksiteD << std::endl;
			} else if (paramName == PARAM_B_RECRUITMENT_TIME) {
				robot_->bRecruitmentTime = paramVal;
				std::cout << paramName << " = " << robot_->bRecruitmentTime << std::endl;
			} else if (paramName == PARAM_B_SCOUTING_PROB) {
				robot_->bScoutingProb = std::atof(paramValRaw.c_str());
				std::cout << paramName << " = " << robot_->bScoutingProb << std::endl;
			} else if (paramName == PARAM_B_MAX_SCOUTING_TIME) {
				robot_->bMaxScoutingTime = paramVal;
				std::cout << paramName << " = " << robot_->bMaxScoutingTime << std::endl;
			} else if (paramName == PARAM_MAX_WORKSITE_D) {
				robot_->maxWorksiteD = paramVal;
				std::cout << paramName << " = " << robot_->maxWorksiteD << std::endl;
			}
		}
	} else {
		std::cout << "!!!! Unable to open file" << std::endl;
	}

	std::cout << "----------------------------------------------------" << std::endl;
}




void IPCInterface::SendResourceRequest(int worksiteId_, double amount_) {
	int dataArray[2];
	dataArray[0] = worksiteId_;
	dataArray[1] = amount_ * 10000;
	ipc.SendData(IPC::RESOURCE_REQUEST, (uint8_t*)dataArray, sizeof(int)*2);
	//std::cout << " >>> SendResourceRequest worksite id" << worksiteId_ << "   amount " << DataArray[1] << " - port " << this->Port << std::endl;
}

void IPCInterface::SendInfoEventReport(InfoEventReport report_) {
	int dataArray[4];
	dataArray[0] = report_.globalTime;
	dataArray[1] = report_.type;
	dataArray[2] = report_.worksiteId;
	dataArray[3] = report_.recruiterId;

	ipc.SendData(IPC::INFO_EVENT_REPORT, (uint8_t*)dataArray, sizeof(int)*4);
	//std::cout << "                        >>> Sending report " << report_.GetDescriptionString() << std::endl;
}

void IPCInterface::SendRecruitmentSignal(int worksiteId_, int worksiteX_, int worksiteY_) {
	int dataArray[3];
	dataArray[0] = worksiteId_;
	dataArray[1] = worksiteX_;
	dataArray[2] = worksiteY_;
	ipc.SendData(IPC::ROBOT_COMM, (uint8_t*)dataArray, sizeof(int)*3);
	if (DEBUG_COMM_SENDING == 1 && worksiteId_ > 0) { std::cout << " >>> " << robot->thisRobotId << " recruiting for W " << worksiteId_ << " x:" << worksiteX_ << " y:" << worksiteY_ << std::endl; }
}

void IPCInterface::ReceiveCallback(const ELolMessage*msg, void *conn, void *ptr){
	IPCInterface *receivingIpcInterface = (IPCInterface*)ptr;
	//std::cout << receivingIpcInterface->robot->thisRobotId << " CALL BACK for type " << msg->command << " msg length" << msg->length << std::endl;
	switch(msg->command) {
		case IPC::ROBOT_COMM: {
			memcpy(receivingIpcInterface->robotCommData,msg->data,msg->length);
			if (DEBUG_COMM_RECEIVING == 1) {
				if (! (receivingIpcInterface->robotCommData[0] == -1)) {
					std::cout << ">>> " << receivingIpcInterface->robot->thisRobotId << " received recruitment data id " << receivingIpcInterface->robotCommData[0];
					std::cout << "x " << receivingIpcInterface->robotCommData[1] << " y" << receivingIpcInterface->robotCommData[2];
					std::cout << " recruiter id " << receivingIpcInterface->robotCommData[3] << std::endl;

				}
			}
		} break;
		case IPC::VICON_DATA: {

			memcpy(receivingIpcInterface->viconData,msg->data,msg->length);
			if (DEBUG_MESSAGE_RECEIVED == 1) {
				std::cout << " Received data of length " << msg->length << ":";
				for (int i=0; i<10; i++) {
					std::cout << receivingIpcInterface->viconData[i] << " ";
				}
				std::cout << std::endl;
			}

			memcpy(receivingIpcInterface->noiselessViconData,msg->data,msg->length);

			//-- simulate noise on viconData
			if (receivingIpcInterface->timeTillNextPositionUpdate <= 0) {
				receivingIpcInterface->timeTillNextPositionUpdate = receivingIpcInterface->robot->timeBetweenGlobalPositionUpdates;
				if (DEBUG_POSITION_NOIZE == 1) {
					std::cout << " received clean: ";
					std::cout << receivingIpcInterface->viconData[0] << " ";
					std::cout << receivingIpcInterface->viconData[1] << " ";
					std::cout << (receivingIpcInterface->viconData[2] / 10000.0)*(180/PI) << " ";
				}

				//-- simulate noise on position data
				receivingIpcInterface->viconData[0] = receivingIpcInterface->viconData[0] + mathHelpers_generateRandGauss(receivingIpcInterface->robot->globalPositionErrorVariance);
				receivingIpcInterface->viconData[1] = receivingIpcInterface->viconData[1] + mathHelpers_generateRandGauss(receivingIpcInterface->robot->globalPositionErrorVariance);
				receivingIpcInterface->viconData[2] = receivingIpcInterface->viconData[2] + mathHelpers_generateRandGauss(receivingIpcInterface->robot->globalRotationErrorVariance*10000*PI / 180);

				if (DEBUG_POSITION_NOIZE == 1) {
					std::cout << " after noize: ";
					std::cout << receivingIpcInterface->viconData[0] << " ";
					std::cout << receivingIpcInterface->viconData[1] << " ";
					std::cout << (receivingIpcInterface->viconData[2] / 10000.0)*(180/PI) << " ";

					std::cout << std::endl;
				}
			} else {
				//-- simulate delay in receiving global position and revert to old data
				receivingIpcInterface->viconData[0] = receivingIpcInterface->prevViconData[0];
				receivingIpcInterface->viconData[1] = receivingIpcInterface->prevViconData[1];
				receivingIpcInterface->viconData[2] = receivingIpcInterface->prevViconData[2];

				if (DEBUG_POSITION_NOIZE == 1) { std::cout << " /simulating delay/" << std::endl; }
			}

			memcpy(receivingIpcInterface->prevViconData,receivingIpcInterface->viconData,msg->length);

		}
		break;

		default:
		  break;
	}
}



int IPCInterface::GetRobotPositionX(){
  return viconData[0];
}

int IPCInterface::GetRobotPositionY(){
  return viconData[1];
}

double IPCInterface::GetRobotPositionA(){
  return viconData[2] / 10000.0; // was sent in multiply of 10000 because only ints were possible
}

int IPCInterface::GetWorksite1Id() {
	return viconData[3];
}

int IPCInterface::GetWorksite1X() {
	return viconData[4];
}

int IPCInterface::GetWorksite1Y() {
	return viconData[5];
}

int IPCInterface::GetResourcePorionId() {
	return viconData[6];
}

double IPCInterface::GetResouceReturned() {
	return viconData[7] / 10000.0; // was sent in multiply of 10000 because only ints were possible
}

double IPCInterface::GetResourceLeft() {
	return viconData[8] / 10000.0; // was sent in multiply of 10000 because only ints were possible
}

int IPCInterface::GetGlobalTime() {
	return viconData[9];
}

int IPCInterface::GetNoiselessRobotPositionX(){
  return noiselessViconData[0];
}

int IPCInterface::GetNoiselessRobotPositionY(){
  return noiselessViconData[1];
}

int IPCInterface::GetNoiselessWorksite1X() {
	return noiselessViconData[4];
}
int IPCInterface::GetNoiselessWorksite1Y() {
	return noiselessViconData[5];
}

//-- recruitment
int IPCInterface::GetRecruitedWorksiteId() {
	return robotCommData[0];
}

int IPCInterface::GetRecruitedWorksiteX() {
	return robotCommData[1];
}

int IPCInterface::GetRecruitedWorksiteY() {
	return robotCommData[2];
}

int IPCInterface::GetRecruiterId() {
	return robotCommData[3];
}

IPCInterface::~IPCInterface(){}
