
#include <iostream>
#include <camera.h>

#include <IPCInterface.h>
#include <PathPlanner.h>
#include <LinuxBoardController.h>
#include <EPuckBoardController.h>
#include <SolitaryActionController.h>
#include <LocalBroadcasterActionController.h>
#include <BeeActionController.h>
#include <ReportTestActionController.h>

#include <constants.h>
#include <Vector2D.h>


#include "epuck.hh"

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


using namespace std;

int g_logtofile(0);
int g_turning_speed(10);


/** Main Robot's class constructor 
   \param str Robot's identifier */
Robot::Robot(const char *str) {

	cout << "======= Robot program started "<< endl;
	srand(time(NULL));

	timestamp = 0;
	Name = str;

	avoid_weightleft[0] = -2.0; avoid_weightleft[1] = -2.0; avoid_weightleft[2] = -1.0; avoid_weightleft[3] = 0;
	avoid_weightleft[4] = 0; avoid_weightleft[5] = 1.0; avoid_weightleft[6] = 2.0; avoid_weightleft[7] = 2.0;

	avoid_weightright[0] = 2.0; avoid_weightright[1] = 2.0; avoid_weightright[2] = 1.0; avoid_weightright[3] = 0;
	avoid_weightright[4] = 0; avoid_weightright[5] = -1.0; avoid_weightright[6] = -2.0; avoid_weightright[7] = -2.0;

	isDoingForaging = false;
	worksiteRadius = 0;
	foragingLoadAmount = 0;
	maxWorksiteD = 0;
	taskAllocationLoadAmount = 0;
	baseRadius = 0;
	basePosition = new Vector2D();
	timeBetweenGlobalPositionUpdates = 0;
	globalPositionErrorVariance = 0;
	globalRotationErrorVariance = 0;
	controllerType = 0;
	reportSendTrials = 0;
	thisRobotId = 0;
	commTime = 0;
	lbNearWorksiteD = 0;
	bRecruitmentTime = 0;
	bScoutingProb = 0;
	bMaxScoutingTime = 0;

	IPCInterface::ProcessParameterFile(this);
    
	//-- create Robot's Devices
	camera = new Camera(320,240);
	epuckBoardController = new EPuckBoardController();

	//-- the three below point to the host computer, the ports are defined in cpp_observer: launch files
	ipcVicon = new IPCInterface("192.168.20.229",12000,this,epuckBoardController);
	ipcToVicon = new IPCInterface("192.168.20.229",10000,this,epuckBoardController);
	ipcComm = new IPCInterface("192.168.20.229",9000,this,epuckBoardController);
	ipcToComm = new IPCInterface("192.168.20.229",8000,this,epuckBoardController);

	pathPlanner = new PathPlanner(this,epuckBoardController, ipcVicon);
	linuxBoardController = new LinuxBoardController();

	if (controllerType == 0) {
		actionController = new SolitaryActionController(this, pathPlanner, epuckBoardController);
	} else if (controllerType == 1) {
		actionController = new LocalBroadcasterActionController(this, pathPlanner, epuckBoardController);
	} else if (controllerType == 2) {
		actionController = new BeeActionController(this, pathPlanner, epuckBoardController);
	} else if (controllerType == 3) {
		actionController = new ReportTestActionController(this, pathPlanner, epuckBoardController);
	}

  	// Initialization of Robot Velocities
  	this->rightWheelVelocity = 0;
  	this->leftWheelVelocity = 0;

  	// reset the pic board
  	linuxBoardController->ResetDSPIC();

  	// indicate with board leds that program is running
  	linuxBoardController->SetBoardLED(1,1);
  	linuxBoardController->SetBoardLED(2,1);


}


Robot::~Robot(){
	cout<< " Destroy robot "<<endl;
	delete camera;
	delete epuckBoardController;
	delete ipcToVicon;
	delete ipcVicon;
	delete ipcComm;
	delete ipcToComm;

	delete pathPlanner;
	delete linuxBoardController;
	delete actionController;

	delete basePosition;
}


bool Robot::Init(){

	std::cout << " Init robot " << thisRobotId << " ..." << std::endl;

	epuckBoardController->Init();
	ipcVicon->Init();
	ipcToVicon->Init();

	ipcComm->Init();
	ipcToComm->Init();

	std::cout << " Init robot " << thisRobotId << " DONE " << std::endl;
	return true;
}

/**
 *  Robot's update process returns true after updating robot's state
 */
bool Robot::Update(uint64_t timeStamp_) {
	timestamp = timeStamp_; // approx 10 update loops per second


	epuckBoardController->Run();

	ipcToVicon->Update(timeStamp_);
	ipcVicon->Update(timeStamp_);
	ipcComm->Update(timeStamp_);
	ipcToComm->Update(timeStamp_);

	//std::cout << " . " << std::endl;
	//-- do update step of the action controller, then get resulting wheel velocities
	BaseActionController::STATE currentState = actionController->ProcessCurrentState(timeStamp_);
	this->SetLEDSForState(currentState, actionController->isCommunicating, timeStamp_);

	if (currentState == BaseActionController::STATE_WATITING_FOR_SERVER) {
		std::cout << " !!!! " << thisRobotId << " waiting for server " << std::endl;
		epuckBoardController->SetSpeeds(0,0);
		return true;
	}

	//-- everything ok, continue
	Vector2D* navVector = actionController->GetCurrentNavigationVector();

	leftWheelVelocity = navVector->x;
	rightWheelVelocity = navVector->y;


	/*leftWheelVelocity = ROBOT_MAX_SPEED;
	rightWheelVelocity = ROBOT_MAX_SPEED;
	BaseActionController::STATE currentState = BaseActionController::STATE_SCOUTING;*/
	//-- in addition to what the action controller says, do lower-level actions like obstacle avoidance
	if (controllerType < 3) {
		if (currentState != BaseActionController::STATE_PROCESSING_WORKSITE) {
			//std::cout << " OA " << std::endl;
			this->DoObstacleAvoidance();
		}
	}


	//std::cout << timestamp << std::endl;

	//-- perform motor control
	/*if (thisRobotId != 37) {
		leftWheelVelocity = 0;
		rightWheelVelocity = 0;
	}*/

	epuckBoardController->SetSpeeds(leftWheelVelocity,rightWheelVelocity);
	//epuckBoardController->SetSpeeds(500,500);

	//std::cout << " wheel speeds: L=" << leftWheelVelocity << " R" << rightWheelVelocity << std::endl;


	return true;
}



/**
 * Robot stops program
 */
void Robot::StopProgram() {
    leftWheelVelocity = 0;
    rightWheelVelocity = 0;
    epuckBoardController->SetSpeeds(leftWheelVelocity,rightWheelVelocity);
    epuckBoardController->Run();

    linuxBoardController->SetBoardLED(1,0);
    linuxBoardController->SetBoardLED(2,0);

    for (int i=0; i<8; i++) {
    	epuckBoardController->SetLED(i,0);
    }
}

/** Robot resets its the SPIComm process  */
bool Robot::Reset(){
    epuckBoardController->Reset();
    epuckBoardController->Run();
    return true;
}

/** Returns a pointer to Robot's Embedded Camera Device  */
Camera* Robot::GetCameraDevice(){
  return camera;
}

/** Returns a pointer to Robot's SPIComm Interface  */
EPuckBoardController* Robot::GetSPICommInterface(){
  return epuckBoardController;
}



/**
 * Robot's Colilsion Avoidance function
 */
void Robot::DoObstacleAvoidance(){
	double maxVal = 2000.0;
    vector<int> IRSENSORSVECTOR = epuckBoardController->GetIRDataVector();
    int leftWheelChange = 0;
    int rightWheelChange = 0;
    int sensorId;

    //leftWheelVelocity = ROBOT_MAX_SPEED;
    //rightWheelVelocity = ROBOT_MAX_SPEED;

    double isVeryNear = false;

    for (int sensorId=0; sensorId<IRSENSORSVECTOR.size(); sensorId++) {

		int CurrentIR = IRSENSORSVECTOR[sensorId];
		double a = (CurrentIR) / maxVal;
		double valPercentage = min(1.0,double(CurrentIR) / maxVal);

		if (valPercentage > 0.5 && (sensorId >= 6 || sensorId <=1)  ) {
			isVeryNear = true;
		}
		//std::cout << " IR" << sensorId << "  " << a << " val " << CurrentIR << " val%" << valPercentage << "   " << std::endl;

		leftWheelChange += avoid_weightleft[sensorId] * valPercentage * ROBOT_MAX_SPEED;
		rightWheelChange += avoid_weightright[sensorId] * valPercentage * ROBOT_MAX_SPEED;

    }
    if (isVeryNear) {
    	leftWheelVelocity = leftWheelChange;
    	rightWheelVelocity = rightWheelChange;
    } else {
    	leftWheelVelocity += leftWheelChange;
    	rightWheelVelocity += rightWheelChange;
    }


    leftWheelVelocity = max(-ROBOT_MAX_SPEED, min(ROBOT_MAX_SPEED,leftWheelVelocity));
    rightWheelVelocity = max(-ROBOT_MAX_SPEED, min(ROBOT_MAX_SPEED,rightWheelVelocity));

    //std::cout << ipcVicon->GetGlobalTime() << " OA " << std::endl;
    //std::cout << ipcVicon->GetGlobalTime() << " !!! AVOIDANCE: LEFT wheel change: " << leftWheelChange << " RIGHT wheel change: " << rightWheelChange << " velocity" << leftWheelVelocity << "  " << rightWheelVelocity << std::endl;

    if (isVeryNear) {
    	for (int i=0; i<8; i++) {
    		epuckBoardController->SetLED(i,1);
    	}
    } else {
    	for (int i=0; i<8; i++) {
    		epuckBoardController->SetLED(0,1);
    	}
    }
}


/*void Robot::TurnLeft(){
  rightWheelVelocity = ROBOT_MAX_SPEED*0.40;
  leftWheelVelocity = 0;
}

void Robot::TurnRight(){
  rightWheelVelocity = 0;
  leftWheelVelocity = ROBOT_MAX_SPEED*0.40;
}*/


/** Prints IR Sensor Vector */
/*void Robot::PrintProximityValues(){

  cout << this->timestamp\
       << " : "\
       << this->Name.c_str()\
       << " IR --[";

  vector<int>* IRSENSORSVECTOR = epuckBoardController->GetIRDataVector();

  for (vector<int>::iterator CurrentIR = IRSENSORSVECTOR->begin();\
       CurrentIR != IRSENSORSVECTOR->end();\
       ++CurrentIR)
    printf(" %d", *CurrentIR);

  cout << "]" << endl;
}*/

/** Prints MIC Values Vector */
void Robot::PrintMicValues(){

  cout << this->timestamp\
       << " : "\
       << this->Name.c_str()\
       << " MIC --[";

  vector<int>* MICROPHONESVECTOR = epuckBoardController->GetMicDataVector();

  for (vector<int>::iterator CurrentMIC = MICROPHONESVECTOR->begin();\
       CurrentMIC != MICROPHONESVECTOR->end();\
       ++CurrentMIC)
    printf(" %d", *CurrentMIC);

  cout << "]" << endl;
}

/** Prints ACC Values Vector */
void Robot::PrintAccelerometersValues(){

  cout << this->timestamp\
       << " : "\
       << this->Name.c_str()\
       << " ACC --[";

  vector<int>* ACCELEROMETERSVECTOR = epuckBoardController->GetAccelerometerDataVector();

  for (vector<int>::iterator CurrentACC = ACCELEROMETERSVECTOR->begin();\
       CurrentACC != ACCELEROMETERSVECTOR->end();\
       ++CurrentACC)
    printf(" %d", *CurrentACC);

  cout << "]" << endl;
}

/** Prints Battery Level */
void Robot::PrintBatteryLevel(){

  int* BatteryLevel  = epuckBoardController->GetBatteryLevel();

  cout << this->timestamp\
       << " : "\
       << this->Name.c_str()\
       << " Bat --[ "\
       << *BatteryLevel\
       << " ] " << endl;
}

/** Prints current robot's state */
void Robot::PrintStatus(){

  printf("%d:%s in state [%s] (%d - %d)\n",	\
	 this->timestamp,			\
	 this->Name.c_str());
}



/* ============================= UTILS ==================== */



void Robot::SetLEDSForState(BaseActionController::STATE state_, bool isCommunicating_,uint64_t timeStamp_) {
	if (isCommunicating_) {
		epuckBoardController->SetLED(0,0);
		epuckBoardController->SetLED(1,0);
		epuckBoardController->SetLED(2,0);
		epuckBoardController->SetLED(3,0);
		epuckBoardController->SetLED(4,0);
		epuckBoardController->SetLED(5,0);
		epuckBoardController->SetLED(6,0);
		epuckBoardController->SetLED(7,0);
		/*if (timeStamp_ % 10 < 5) {
			epuckBoardController->SetLED(6,0);
			epuckBoardController->SetLED(2,0);
		} else {
			epuckBoardController->SetLED(6,1);
			epuckBoardController->SetLED(2,1);
		}*/
		int ledId = timeStamp_ % 10;
		if (ledId >= 0 && ledId <= 7) {
			epuckBoardController->SetLED(ledId,1);
		}

	} else {
		switch (state_) {
			case BaseActionController::STATE_WATITING_FOR_SERVER:
				epuckBoardController->SetLED(0,1);
				epuckBoardController->SetLED(1,1);
				epuckBoardController->SetLED(2,1);
				epuckBoardController->SetLED(3,1);
				epuckBoardController->SetLED(4,1);
				epuckBoardController->SetLED(5,1);
				epuckBoardController->SetLED(6,1);
				epuckBoardController->SetLED(7,1);
				break;
			case BaseActionController::STATE_RESTING:
				epuckBoardController->SetLED(0,0);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,0);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,0);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,0);
				epuckBoardController->SetLED(7,0);
				break;
			case BaseActionController::STATE_SCOUTING:
				epuckBoardController->SetLED(0,1);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,0);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,0);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,0);
				epuckBoardController->SetLED(7,0);
				break;
			case BaseActionController::STATE_GOING_TO_WORKSITE:
				epuckBoardController->SetLED(0,1);
				epuckBoardController->SetLED(1,1);
				epuckBoardController->SetLED(2,0);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,0);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,0);
				epuckBoardController->SetLED(7,1);
				break;
			case BaseActionController::STATE_PROCESSING_WORKSITE:
				epuckBoardController->SetLED(0,0);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,1);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,0);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,1);
				epuckBoardController->SetLED(7,0);
				break;
			case BaseActionController::STATE_RETURN_TO_BASE:
				epuckBoardController->SetLED(0,0);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,1);
				epuckBoardController->SetLED(3,1);
				epuckBoardController->SetLED(4,1);
				epuckBoardController->SetLED(5,1);
				epuckBoardController->SetLED(6,1);
				epuckBoardController->SetLED(7,0);
				break;
			case BaseActionController::STATE_RETURN_TO_BASE_UNSUCCESSFUL:
			case BaseActionController::STATE_RETURN_TO_BASE_UNSUCCESSFUL_BLIND:
				epuckBoardController->SetLED(0,0);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,1);
				epuckBoardController->SetLED(3,1);
				epuckBoardController->SetLED(4,0);
				epuckBoardController->SetLED(5,1);
				epuckBoardController->SetLED(6,1);
				epuckBoardController->SetLED(7,0);
				break;
			case BaseActionController::STATE_UNLOADING_RESOURCE:
				epuckBoardController->SetLED(0,0);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,1);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,1);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,1);
				epuckBoardController->SetLED(7,0);
				break;

			case BaseActionController::STATE_BEE_OBSERVING:
				epuckBoardController->SetLED(0,1);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,0);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,1);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,0);
				epuckBoardController->SetLED(7,0);
				break;
			case BaseActionController::STATE_BEE_RECRUITER_WAGGLE_DANCING:
				epuckBoardController->SetLED(0,1);
				epuckBoardController->SetLED(1,0);
				epuckBoardController->SetLED(2,1);
				epuckBoardController->SetLED(3,0);
				epuckBoardController->SetLED(4,1);
				epuckBoardController->SetLED(5,0);
				epuckBoardController->SetLED(6,1);
				epuckBoardController->SetLED(7,0);
				break;


		}
	}
}

