/*
 * BaseActionController.cpp
 *
 *  Created on: 29 Mar 2017
 *      Author: lenka
 */

#include <constants.h>
#include <epuck.hh>

#include <EPuckBoardController.h>
#include <Vector2D.h>
#include <IPCInterface.h>
#include "PathPlanner.h"

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



BaseActionController::BaseActionController(Robot* robot_, PathPlanner* pathPlanner_, EPuckBoardController* epuckBoardController_) {
	robot = robot_;
	pathPlanner = pathPlanner_;
	epuckBoardController = epuckBoardController_;
	randomWalkTimeStep = 0;
	currentState = STATE_SCOUTING;
	currentNavigationVector = new Vector2D();
	targetWorksiteInfo = WorksiteInfo();
	resourceRequestsCounter = 1;
	didStartEarningReward = false;
	didReportReportTaskStartedEvent = false;
	didReportWorksiteReachedEvent = false;
	reportCopiesSent = 0;
	rewardReceived = 0;
	timeGoingToWorksite = 0;
	timeTillNextScoutAllowed = 10; //approx. 10 ticks per second
	timeGlobalTimeTheSame = 0;
	lastGlobalTime = 0;
	totalResourceCollected = 0;
	isCommunicating = false;

	timeSinceRecruited = -1;
	recruitedWorksiteId = -1;
	recruitedWorksiteX = -1;
	recruitedWorksiteY = -1;
	recruiterId = -1;

	if (USE_REMOTE_REPORTING != 1) {
		std::ostringstream filePath;
		bool fileNameFound = false;
		int counter = 0;
		while (!fileNameFound) {
			filePath.str("");
			filePath.clear();
			filePath << "/root/code_lenka/log/" << robot->logFilePrefix << "_infoEvents" << counter << ".txt";
			if (std::ifstream(filePath.str().c_str())) {
			     counter++;
			} else {
				std::cout << " ~~~~~~~ [R" << robot->thisRobotId << "] log file: " << counter << ":" << filePath.str().c_str() << std::endl;
				fileNameFound = true;
			}
		}

		outputFileInfoEvents.open(filePath.str().c_str(), std::ios_base::trunc | std::ios_base::out);
		//outputFileInfoEvents << "time\tevtType\t\trobotId\tsiteId\tsiteX\tsiteY\tsiteVol\trecruiterId\treceivedTime" << std::endl;
	}
}

BaseActionController::~BaseActionController() {
	delete currentNavigationVector;
}

/**
 * Set state and reset relevant variables
 */
void BaseActionController::SetState(STATE state_) {
	currentState = state_;
	std::cout << "      [R" << robot->thisRobotId << "] new state: ";
	switch (state_) {
		case BaseActionController::STATE_WATITING_FOR_SERVER:
			std::cout << "STATE_WATITING_FOR_SERVER";
			break;
		case BaseActionController::STATE_RESTING:
			std::cout << "STATE_RESTING";
			break;
		case BaseActionController::STATE_SCOUTING:
			std::cout << "STATE_SCOUTING";
			break;
		case BaseActionController::STATE_GOING_TO_WORKSITE:
			std::cout << "STATE_GOING_TO_WORKSITE";
			break;
		case BaseActionController::STATE_PROCESSING_WORKSITE:
			std::cout << "STATE_PROCESSING_WORKSITE";
			break;
		case BaseActionController::STATE_RETURN_TO_BASE:
			std::cout << "STATE_RETURN_TO_BASE";
			break;
		case BaseActionController::STATE_RETURN_TO_BASE_UNSUCCESSFUL:
		case BaseActionController::STATE_RETURN_TO_BASE_UNSUCCESSFUL_BLIND:
			std::cout << "STATE_RETURN_TO_BASE_UNSUCCESSFUL";
			break;
		case BaseActionController::STATE_UNLOADING_RESOURCE:
			std::cout << "STATE_UNLOADING_RESOURCE";
			break;
		case BaseActionController::STATE_BEE_OBSERVING:
			std::cout << "STATE_BEE_OBSERVING";
			break;
		case BaseActionController::STATE_BEE_RECRUITER_WAGGLE_DANCING:
			std::cout << "STATE_BEE_RECRUITER_WAGGLE_DANCING";
			break;
	}
	std::cout << std::endl;

	if (state_ == STATE_SCOUTING) {
		didReportReportTaskStartedEvent = false;
		didReportWorksiteReachedEvent = false;
		timeSinceRecruited = -1;
	} else if (state_ == STATE_GOING_TO_WORKSITE) {
		didStartEarningReward = false;
		rewardReceived = 0;
		timeGoingToWorksite = 0;
		timeTillNextScoutAllowed = 0;
	}
}

/**
 * Perform actions of the current state and decide on the next state.
 * @return the current state, after the function has run
 */
BaseActionController::STATE BaseActionController::ProcessCurrentState(uint64_t timeStamp_) {
	//std::cout << " --- update " << ipcVicon->GetGlobalTime() << " state:" << currentState << "  pos " << pathPlanner->GetGlobalPosition()->GetOutputString() << "last gt" << lastGlobalTime << std::endl;
	//std::cout << robot->thisRobotId << " . " << std::endl;
	//-- check global time didn't stop - if yes, the robot should wait
	if (robot->ipcVicon->GetGlobalTime() == lastGlobalTime) {
		timeGlobalTimeTheSame++;
		if (timeGlobalTimeTheSame > 50) { //normally it should take about 10 ticks of robot for global time to change
			//std::cout << " !!!! WAITING FOR SERVER " << std::endl;
			return STATE_WATITING_FOR_SERVER;
		}

	} else {
		timeGlobalTimeTheSame = 0;
	}
	lastGlobalTime = robot->ipcVicon->GetGlobalTime();

	//-- update path planner
	pathPlanner->OnRobotPositionUpdated();

	if (currentState == STATE_SCOUTING) {
		this->Scout();

		/*if (pathPlanner->GetHasValidData()) {
			targetWorksiteInfo.id = 1;
			//pathPlanner->SetRelVectorToWorksiteFromGlobalCoords(0,0);
			pathPlanner->globalWorksiteCoord = new Vector2D(0,0);
			//std::cout << "     !!!!!!! WORKSITE SCOUTED global vec: " <<  pathPlanner->globalWorksiteCoord->GetOutputString() << std::endl;
			NewInfoEventReport(InfoEventReport::TYPE_WORKSITE_SCOUTED,targetWorksiteInfo.id,-1);
			SetState(STATE_GOING_TO_WORKSITE);
		}*/

	} else if (currentState == STATE_GOING_TO_WORKSITE) {
		this->GoToWorksite();
	} else if (currentState == STATE_RETURN_TO_BASE || currentState == STATE_RETURN_TO_BASE_UNSUCCESSFUL || currentState == STATE_RETURN_TO_BASE_UNSUCCESSFUL_BLIND) {
		this->ReturnToBase();
	} else if (currentState == STATE_PROCESSING_WORKSITE) {
		ProcessWorksite();
	}

	//-- reporting
	SendReportsToServer();
	if (robot->ipcVicon->GetGlobalTime() % 100 == 0 && timeStamp_ % 10 == 0) {
		std::cout << "[R" << robot->thisRobotId << "] total res " << totalResourceCollected << std::endl;
	}

	//currentNavigationVector->x = ROBOT_MAX_SPEED;
	//currentNavigationVector->y = ROBOT_MAX_SPEED;
	return currentState;
}

/**
 * Send the current report to the server computer, using the report queue
 */
void BaseActionController::SendReportsToServer() {
	if (USE_REMOTE_REPORTING == 1) {
		//-- send report from the end of the queue
		if (infoEventReports.size() > 0) {
			robot->ipcToVicon->SendInfoEventReport(infoEventReports[infoEventReports.size()-1]);
			reportCopiesSent++;
			if (reportCopiesSent > robot->reportSendTrials) { //try to send each report N times, to make sure it gets to the server
				infoEventReports.pop_back();
				reportCopiesSent = 0;
			}
		} else {
			//-- nothing to report, send empty
			robot->ipcToVicon->SendInfoEventReport(emptyInfoEventReport);
		}
	}
}

/* ======================== SCOUTING =====================*/
void BaseActionController::Scout() {

	if (pathPlanner->GetIsRobotInsideBase()) {
		//-- get out of the base
		currentNavigationVector = pathPlanner->GetNavigationVectorAwayFromBase();

	} else {
		CalculateWheelVelocitiesForRandomWalk();

		//-- test if outside of arena size
		//std::cout << robot->thisRobotId << " d to base " << pathPlanner->GetDistanceToBase();
		if (pathPlanner->GetDistanceToBase() > robot->maxWorksiteD + 2000) {
			currentNavigationVector = pathPlanner->GetNavigationVectorToBase();
			//std::cout << " !!" << std::endl;
		}
		//std::cout << std::endl;
		//-- test if at a worksite
		//std::cout <<  " pos " << robot->ipcVicon->GetRobotPositionX() << " next scout at " << timeTillNextScoutAllowed;
		//std::cout << " worksite data Id: " << robot->ipcVicon->GetWorksite1Id() << " X" << robot->ipcVicon->GetWorksite1X() << " y " << robot->ipcVicon->GetWorksite1Y() << std::endl;

		if (timeSinceRecruited < 0) { //ignore any scouting data if already being recruited
			if (robot->ipcVicon->GetWorksite1Id() > 0 && robot->ipcVicon->GetWorksite1Id() < 1000 && timeTillNextScoutAllowed <= 0) {
				if ( !( robot->ipcVicon->GetNoiselessRobotPositionX() == 0 && robot->ipcVicon->GetNoiselessRobotPositionY() == 0 )) {
					this->OnWorksiteScouted(robot->ipcVicon->GetWorksite1Id());
				}
			}
		}
	}

	if (timeTillNextScoutAllowed > 0) {
		timeTillNextScoutAllowed--;
	}
}

/**
 * Called once when robot starts sensing a worksite.
 * Processes the worksite info.
 */
void BaseActionController::OnWorksiteScouted(int worksiteId_) {

	targetWorksiteInfo.id = worksiteId_;

	//-- add error to the global position of spotted worksite by adding the difference between robot's estimated and real position
	Vector2D noiselessPosition = Vector2D(robot->ipcVicon->GetNoiselessRobotPositionX(),robot->ipcVicon->GetNoiselessRobotPositionY());
	Vector2D positionError = Vector2D(pathPlanner->GetGlobalPosition()->x - noiselessPosition.x, pathPlanner->GetGlobalPosition()->y - noiselessPosition.y);
	Vector2D noiselessWorksitePosition = Vector2D(robot->ipcVicon->GetNoiselessWorksite1X(), robot->ipcVicon->GetNoiselessWorksite1Y());

	pathPlanner->globalWorksiteCoord = new Vector2D(noiselessWorksitePosition.x + positionError.x, noiselessWorksitePosition.y + positionError.y);
	targetWorksiteInfo.posX = pathPlanner->globalWorksiteCoord->x;
	targetWorksiteInfo.posY = pathPlanner->globalWorksiteCoord->y;

	/*std::cout << "     !!!!!!! WORKSITE SCOUTED ID " << worksiteId_ << " robot real pos " << noiselessPosition.GetOutputString() << " believed pos " << pathPlanner->GetGlobalPosition()->GetOutputString();
	std::cout << "        worksite real pos " << noiselessWorksitePosition.GetOutputString() << " believed pos " << pathPlanner->globalWorksiteCoord->GetOutputString() << std::endl;
	std::cout << " !!!" << noiselessWorksitePosition.GetOutputString() << std::endl;*/
	NewInfoEventReport(InfoEventReport::TYPE_WORKSITE_SCOUTED,targetWorksiteInfo.id,-1);

	this->SetState(STATE_GOING_TO_WORKSITE);

}

/**
 * Called once when robot starts sensing a worksite.
 * Processes the worksite info.
 */
void BaseActionController::OnRecruitedToWorksite(int worksiteId_, int worksiteX_, int worksiteY_, int recruiterId_) {

	targetWorksiteInfo.id = worksiteId_;

	pathPlanner->globalWorksiteCoord = new Vector2D(worksiteX_, worksiteY_);
	targetWorksiteInfo.posX = pathPlanner->globalWorksiteCoord->x;
	targetWorksiteInfo.posY = pathPlanner->globalWorksiteCoord->y;

	/*std::cout << "     !!!!!!! WORKSITE SCOUTED ID " << worksiteId_ << " robot real pos " << noiselessPosition.GetOutputString() << " believed pos " << pathPlanner->GetGlobalPosition()->GetOutputString();
	std::cout << "        worksite real pos " << noiselessWorksitePosition.GetOutputString() << " believed pos " << pathPlanner->globalWorksiteCoord->GetOutputString() << std::endl;
	std::cout << " !!!" << noiselessWorksitePosition.GetOutputString() << std::endl;*/
	NewInfoEventReport(InfoEventReport::TYPE_ROBOT_RECRUITED,targetWorksiteInfo.id, recruiterId_);

	this->SetState(STATE_GOING_TO_WORKSITE);

}

/* =================== GOING TO WORKSITE =====================*/

/**
 * Navigate to a remembered worksite, using remembered worksite location (in local coordinates)
 */
void BaseActionController::GoToWorksite() {
	//std::cout << "--";
	if (this->GetIsAtWorksite()) {
		//-- the robot believes that it is at the worksite (based on its path planner). Try requesting reward from it
		//std::cout << "!!!!!!!!!!! GOT TO WORKSITE" << std::endl;
		currentNavigationVector = new Vector2D();
		didReportWorksiteReachedEvent = false;
		SetState(STATE_PROCESSING_WORKSITE);
	} else {
		currentNavigationVector = pathPlanner->GetNavigationVectorToWorksite();
		//currentNavigationVector->x = ROBOT_MAX_SPEED;
		//currentNavigationVector->y = ROBOT_MAX_SPEED;
	}

	timeGoingToWorksite++;
	//std::cout << timeGoingToWorksite << std::endl;
	if (timeGoingToWorksite > 1200) { // 1200: it takes robot about 300 ticks to do a straight line across the arena
		//-- got lost or got stuck
		OnUnableToReachWorksite();
	}

}

/**
 * Triggered when robot reached current worksite location and worksite can be processed
 */
void BaseActionController::OnWorksiteReached() {
	NewInfoEventReport(InfoEventReport::TYPE_WORKSITE_REACHED,targetWorksiteInfo.id,-1); // any other info is unknown, will be obtained by the environment
}

/**
 * Trigerred when robot took too long to reach worksite
 */

void BaseActionController::OnUnableToReachWorksite() {
	AbandonWorksite();
	timeTillNextScoutAllowed = 50;
	SetState(STATE_SCOUTING);
}

/* =================== PROCESSING WORKSITE ===================*/

void BaseActionController::ProcessWorksite() {

	//-- stop the robot
	currentNavigationVector->x = 0; currentNavigationVector->y = 0;

	rewardReceived = robot->ipcVicon->GetResouceReturned();
	int portionId = robot->ipcVicon->GetResourcePorionId();
	bool shouldKeepRequesting = true;

	//-- report the event
	if (!didReportWorksiteReachedEvent && rewardReceived > 0) {
		OnWorksiteReached();
		didReportWorksiteReachedEvent = true;
	}

	if (!didReportReportTaskStartedEvent && rewardReceived > 0) {
		//SetNewTarget(taskType_, taskId_, taskValue_, CVector2(0,0), id, targetTaskInfo.taskUtility, 1, numOfRobotsRequired_);
		NewInfoEventReport(InfoEventReport::TYPE_WORKSITE_STARTED,targetWorksiteInfo.id,-1);
		didReportReportTaskStartedEvent = true;

		if (!robot->isDoingForaging) {
			didStartEarningReward = true;
			NewInfoEventReport(InfoEventReport::TYPE_REWARD_STARTED, targetWorksiteInfo.id,-1);
		}
	}

	//-- process task
	if (DEBUG_WORKSITE_PROCESSING == 1) { std::cout << " position: " << pathPlanner->GetGlobalPosition()->GetOutputString() << " received amount " << rewardReceived << " for portion id " << portionId << std::endl; }

	if (portionId == resourceRequestsCounter) {
		if (rewardReceived > MIN_REWARD_NO) {

			//std::cout << " GOT " << rewardReceived << " reward for portion id " << portionId << " amount left" << robot->ipcVicon->GetResourceLeft() <<  std::endl;

			if (!robot->isDoingForaging) {
				//-- task allocation: tell server reward received & request more
				NewInfoEventReport(InfoEventReport::TYPE_REWARD_RECEIVED, resourceRequestsCounter, rewardReceived*10000); //instead of worksite id, send request counter so that more than 1 report of reward received for the same time can be done. Borrow the place for recruiter id to report amount received, this is ok because this event doesn't go to event log
				resourceRequestsCounter ++;
				totalResourceCollected += rewardReceived;
				//std::cout << "R" << robot->thisRobotId << " total res:" << totalResourceCollected << std::endl;
			} else {
				//-- foraging: return to base
				resourceRequestsCounter ++;
				shouldKeepRequesting = false;
				SetState(STATE_RETURN_TO_BASE);
				if (robot->ipcVicon->GetResourceLeft() < 0.01) {
					//-- nothing left, abandon
					OnWorksiteProcessingDone();
				} else {
					//-- pause and will return later
					NewInfoEventReport(InfoEventReport::TYPE_WORKSITE_PAUSED, targetWorksiteInfo.id, -1);
				}


			}


		} else {
			//-- no more left in worksite
			resourceRequestsCounter ++;
			shouldKeepRequesting = false;
			SetState(STATE_SCOUTING);
			OnWorksiteProcessingDone();
		}
	} else {
		//-- got back wrong portion id - keep doing whatever it's doing
	}

	//-- keep requesting
	if (shouldKeepRequesting) {
		if (DEBUG_WORKSITE_PROCESSING == 1) { std::cout << " got back portion id " << portionId << "  requesting for portion id = " << resourceRequestsCounter << std::endl; }
		if (robot->isDoingForaging) {
			robot->ipcToVicon->SendResourceRequest(targetWorksiteInfo.id*1000 + resourceRequestsCounter, robot->foragingLoadAmount);
		} else {
			robot->ipcToVicon->SendResourceRequest(targetWorksiteInfo.id*1000 + resourceRequestsCounter, robot->taskAllocationLoadAmount);
		}
	}



}

/**
 * Trigerred when a worksite has been depleted
 */
void BaseActionController::OnWorksiteProcessingDone() {
	AbandonWorksite();
}

/**
 * Abandon a task
 */
void BaseActionController::AbandonWorksite() {

	NewInfoEventReport(InfoEventReport::TYPE_WORKSITE_ABANDONED, targetWorksiteInfo.id,-1);

	if (!robot->isDoingForaging && this->didStartEarningReward) {
		NewInfoEventReport(InfoEventReport::TYPE_REWARD_FINISHED, targetWorksiteInfo.id, -1);
	}

	//std::cout << "!! ABANDONED t" << targetWorksiteInfo.id << std::endl;
	targetWorksiteInfo.id = 0;
}

/* =================== GOING TO BASE =========================*/
void BaseActionController::ReturnToBase() {
	//-- test if should unload or keep going
	if (pathPlanner->GetIsRobotInsideBase()) {
		//- in unloading bay, unload and proceedSendInfoEventReport
		std::cout << " INSIDE BASE" << std::endl;

		if (rewardReceived > 0.01) {
			NewInfoEventReport(InfoEventReport::TYPE_REWARD_RECEIVED, resourceRequestsCounter, rewardReceived*10000); //instead of worksite id, send request counter so that more than 1 report of reward received for the same time can be done. Borrow the place for recruiter id to report amount received, this is ok because this event doesn't go to event log
			NewInfoEventReport(InfoEventReport::TYPE_REWARD_STARTED, targetWorksiteInfo.id, -1);
			NewInfoEventReport(InfoEventReport::TYPE_REWARD_FINISHED, targetWorksiteInfo.id, -1);
			totalResourceCollected += rewardReceived;
			std::cout << "R" << robot->thisRobotId << " total res:" << totalResourceCollected << std::endl;
		}



		if (targetWorksiteInfo.id > 0) {
			//std::cout << " !!!! returning to worksite " << targetWorksiteInfo.id << std::endl;
			this->SetState(STATE_GOING_TO_WORKSITE);
		} else {
			//std::cout << " !!!! no worksite in memory" << std::endl;
			this->SetState(STATE_SCOUTING);
		}
		OnUnloadResourceDone();
	} else {
		//-- still going to the base
		currentNavigationVector = pathPlanner->GetNavigationVectorToBase();

		//-- test if found a deposit - only when foraging was unsuccessful
		if (currentState == STATE_RETURN_TO_BASE_UNSUCCESSFUL) {
			if (robot->ipcVicon->GetWorksite1Id() > 0) {
				this->OnWorksiteScouted(robot->ipcVicon->GetWorksite1Id());
			}
		}
	}
}

/**
 * Triggered once when all resource has been unloaded
 */
void BaseActionController::OnUnloadResourceDone() {

}



/* ========================= VARIOUS HELPER ===================*/
/**
 * Checks if there is recruitment signals around and follows one after waiting a while
 */
void BaseActionController::ProcessRecruitmentSignals() {
	//-- get recruited
	if (timeSinceRecruited < 0) {
		if (robot->ipcComm->GetRecruitedWorksiteId() > 0 && robot->ipcComm->GetRecruitedWorksiteId() < 1000 ) {
			if (DEBUG_COMM_RECEIVING == 1) { std::cout << "[R" << robot->thisRobotId << "] recruited comm started " << std::endl; }
			timeSinceRecruited = 0;

			recruitedWorksiteId = robot->ipcComm->GetRecruitedWorksiteId();
			recruitedWorksiteX = robot->ipcComm->GetRecruitedWorksiteX();
			recruitedWorksiteY = robot->ipcComm->GetRecruitedWorksiteY();
			recruiterId = robot->ipcComm->GetRecruiterId();
		}
	}

	//-- wait after recruited to simulate communication takes time
	isCommunicating = false;
	if (timeSinceRecruited >= 0) {
		isCommunicating = true;
		//std::cout << robot->thisRobotId << ":" << timeSinceRecruited << std::endl;
		if (timeSinceRecruited >= robot->commTime) {
			timeSinceRecruited = -1;
			OnRecruitedToWorksite(recruitedWorksiteId, recruitedWorksiteX, recruitedWorksiteY, recruiterId );
		} else {
			timeSinceRecruited++;
			currentNavigationVector->x = 0;
			currentNavigationVector->y = 0;
		}
	}
}


void BaseActionController::CalculateWheelVelocitiesForRandomWalk() {
	if(randomWalkTimeStep==15) {
		int minSpeed = ROBOT_MAX_SPEED/2;
		int range = ROBOT_MAX_SPEED - minSpeed + 1;
	    currentNavigationVector->x = rand() % range + minSpeed;
	    currentNavigationVector->y = rand() % range + minSpeed;

	    randomWalkTimeStep = 0;


	} else {
		randomWalkTimeStep++;
	}
}

bool BaseActionController::GetIsAtWorksite() {
	//std::cout << " D to worksite " << pathPlanner->GetDistanceToWorksite() << std::endl;
	return (pathPlanner->GetDistanceToWorksite() < robot->worksiteRadius);
}

/**
 * Adds new info event report to the beginning of the infoEventReports array
 */
void BaseActionController::NewInfoEventReport(InfoEventReport::TYPE type_, int worksiteId_, int recruiterId_) {
	NewInfoEventReport(robot->ipcVicon->GetGlobalTime(),type_,worksiteId_,recruiterId_);
}

void BaseActionController::NewInfoEventReport(int globalTime_, InfoEventReport::TYPE type_, int worksiteId_, int recruiterId_) {
	InfoEventReport newReport;
	newReport.globalTime = globalTime_;
	newReport.type = type_;
	newReport.worksiteId = worksiteId_;
	newReport.recruiterId = recruiterId_;

	if (USE_REMOTE_REPORTING == 1) {
		infoEventReports.insert(infoEventReports.begin(), newReport);
	} else {
		outputFileInfoEvents << newReport.globalTime << "\t" << newReport.GetTypeDescriptionString() << "\t" << robot->thisRobotId << "\t" \
							<< newReport.worksiteId << "\t-1\t-1\t-1\t" << newReport.recruiterId << "\t-1" << std::endl;
	}
	std::cout << " ~~~~~~~[R" << robot->thisRobotId << "] INFO EVENT: " << newReport.GetDescriptionString() << std::endl;
}

