/*
 * BeeActionController.cpp
 *
 *  Created on: 5 Jun 2017
 *      Author: lenka
 */

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <iostream>

#include <epuck.hh>
#include <constants.h>
#include <Vector2D.h>
#include <IPCInterface.h>
#include <PathPlanner.h>

#include "../../../cpp_common/mathHelpers.h"

#include "BeeActionController.h"

BeeActionController::BeeActionController(Robot* robot_, PathPlanner* pathPlanner_, EPuckBoardController* epuckBoardController_) :
	BaseActionController(robot_,pathPlanner_,epuckBoardController_) {

	std::cout << "Bee action controller created" << std::endl;

	timeScoutingLeft = 0;// robot->bMaxScoutingTime;
	timeRecruitingLeft = 0;

	/*if (robot->thisRobotId != 3) {
		timeScoutingLeft = 0;
	}*/
}

BeeActionController::~BeeActionController() {

}

void BeeActionController::SetState(BaseActionController::STATE state_) {
	BaseActionController::SetState(state_);

	if (state_ == BaseActionController::STATE_SCOUTING) {
		timeScoutingLeft = robot->bMaxScoutingTime;
	} else if (state_ == BaseActionController::STATE_BEE_RECRUITER_WAGGLE_DANCING) {
		timeRecruitingLeft = robot->bRecruitmentTime;
	} else if (state_ == BaseActionController::STATE_BEE_OBSERVING) {
		timeSinceRecruited = -1;
	}
}

BaseActionController::STATE BeeActionController::ProcessCurrentState(uint64_t timeStamp_) {

	BaseActionController::ProcessCurrentState(timeStamp_);

	if (currentState == STATE_BEE_OBSERVING) {
		Observe();
	} else if (currentState == STATE_BEE_RECRUITER_WAGGLE_DANCING) {
		MoveInWaggleDance();
	}

	//-- recruit if waggle dancing
	if (currentState == STATE_BEE_RECRUITER_WAGGLE_DANCING) {
		robot->ipcToComm->SendRecruitmentSignal(targetWorksiteInfo.id, targetWorksiteInfo.posX, targetWorksiteInfo.posY);
	} else {
		robot->ipcToComm->SendRecruitmentSignal(-1,-1,-1);
	}

	return currentState;
}

void BeeActionController::Scout() {
	BaseActionController::Scout();

	//-- return to base if unsuccessful
	if (timeScoutingLeft <= 0) {
		SetState(BaseActionController::STATE_RETURN_TO_BASE_UNSUCCESSFUL);
	}
	timeScoutingLeft--;
}

void BeeActionController::OnWorksiteScouted(int worksiteId_) {
	BaseActionController::OnWorksiteScouted(worksiteId_);

	//-- during task allocation, successful scout first returns to the base to recruit
	if (!robot->isDoingForaging) {
		SetState(BaseActionController::STATE_RETURN_TO_BASE);
	}
}

void BeeActionController::OnUnloadResourceDone() {
	BaseActionController::OnUnloadResourceDone();
	if (targetWorksiteInfo.id == 0) {
		SetState(STATE_BEE_OBSERVING);
	} else {
		SetState(STATE_BEE_RECRUITER_WAGGLE_DANCING);
	}

}


/**
 * Trigerred when a worksite has been depleted
 */
void BeeActionController::OnWorksiteProcessingDone() {
	BaseActionController::OnWorksiteProcessingDone();
	//-- base robot will go scouting when a worksite is done, but bee robot should return to the base to become an observer
	if (currentState == STATE_SCOUTING) {
		SetState(STATE_RETURN_TO_BASE_UNSUCCESSFUL);
	}
}

//====================================== BEE-SPECIFIC

/**
 * Listen to waggle dances in the base
 */

void BeeActionController::Observe() {
	//-- make sure it is in the base
	if (!pathPlanner->GetIsRobotInsideBase()) {
		//std::cout << robot->thisRobotId << " out side" << std::endl;
		currentNavigationVector = pathPlanner->GetNavigationVectorToBase();
	} else {
		//-- slow random walk across base
		//std::cout << robot->thisRobotId << " inside" << std::endl;
		CalculateWheelVelocitiesForRandomWalk();
	}

	BaseActionController::ProcessRecruitmentSignals();

	//-- scout with scouting probability
	if (currentState == BaseActionController::STATE_BEE_OBSERVING) {
		double r = mathHelpers_generateRand(0,1);
		//std::cout << r << std::endl;
		if (r < robot->bScoutingProb) {
			SetState(STATE_SCOUTING);
		}
	}
}


/**
 * Waggle dance by moving randomly across the bases
 */

void BeeActionController::MoveInWaggleDance() {
	//-- make sure it is in the base
	if (!pathPlanner->GetIsRobotInsideBase()) {
		currentNavigationVector = pathPlanner->GetNavigationVectorToBase();
	} else {
		//-- slow random walk across base
		CalculateWheelVelocitiesForRandomWalk();
	}

	if (timeRecruitingLeft <= 0) {
		SetState(STATE_GOING_TO_WORKSITE);
	}
	timeRecruitingLeft--;
}
