
#include <IPCInterface.h>
#include <iostream>


#include <Vector2D.h>
#include <constants.h>
#include <epuck.hh>
#include <EPuckBoardController.h>
#include "../../../cpp_common/mathHelpers.h"

#include <PathPlanner.h>


PathPlanner::PathPlanner(Robot* robot_, EPuckBoardController* boardController_, IPCInterface *IPCROSVICON_){

	robot = robot_;
	IPCROSVICON = IPCROSVICON_;
	epuckBoardController = boardController_;

	relVectorToBase = new Vector2D(0,0);

	relVectorToWorksite = NULL;
	relAccelerationVector = new Vector2D(0,0);
	prevGlobalPosition = NULL;
	currentGlobalPosition = new Vector2D(0,0);
	prevGlobalRotation = 0;
	globalWorksiteCoord = NULL;
	angleToTargetChangeCounter = 0;

	rotateTowardsTargetBehindByLeft = (mathHelpers_generateRand(0,1) > 0.5);
}

PathPlanner::~PathPlanner(){
	delete relVectorToBase;
	if (relVectorToWorksite != NULL) { delete relVectorToWorksite; }
	delete relAccelerationVector;
	if (prevGlobalPosition != NULL) { delete prevGlobalPosition; }
	delete currentGlobalPosition;
	if (globalWorksiteCoord != NULL) { delete globalWorksiteCoord; }
}

/**
 * Makes the relative vector to worksite invalid
 */
void PathPlanner::ForgetWorksite() {
	relVectorToWorksite = NULL;
}

/**
 * Recalculate internally stored vector. Should be called every time robot moves.
 */
void PathPlanner::OnRobotPositionUpdated() {

	if (this->GetHasValidData()) {
		currentGlobalPosition->x = IPCROSVICON->GetRobotPositionX();
		currentGlobalPosition->y = IPCROSVICON->GetRobotPositionY();
		relVectorToBase = this->CalculateRelativeVectorTo(robot->basePosition);


		//std::cout << " current pos " << currentGlobalPosition->GetOutputString() << std::endl;

		//-- odometry
		if (prevGlobalPosition != NULL) {

			if (DEBUG_ODOMETRY) { std::cout << "- Cur position " << currentGlobalPosition->GetOutputString() << " rot " << IPCROSVICON->GetRobotPositionA() *180/PI << "  prev position" << prevGlobalPosition->GetOutputString() << " rot " << prevGlobalRotation *180/PI; }

			relAccelerationVector = this->CalculateRelativeVectorTo(prevGlobalPosition);
			relAccelerationVector->x = -relAccelerationVector->x;
			relAccelerationVector->y = -relAccelerationVector->y;

			if (DEBUG_ODOMETRY) { std::cout << " ACC VECTOR " << relAccelerationVector->GetOutputString(true); }
		} else {
			prevGlobalPosition = new Vector2D();
		}


		if (globalWorksiteCoord != NULL) {
			relVectorToWorksite = this->CalculateRelativeVectorTo(globalWorksiteCoord, true);
		}


		prevGlobalPosition->x = IPCROSVICON->GetRobotPositionX();
		prevGlobalPosition->y = IPCROSVICON->GetRobotPositionY();
		prevGlobalRotation = IPCROSVICON->GetRobotPositionA();
	}


}

/**
 * Returns a vector by which wheels should move in order to navigate to base, where
 * X is left wheel velocity
 * Y is right wheel velocity
 */
Vector2D* PathPlanner::GetNavigationVectorToBase() {
	return this->GetNavigationVectorTo(relVectorToBase);
}


/**
 * Returns a vector by which wheels should move in order to navigate to remembered worksite, where
 * X is left wheel velocity
 * Y is right wheel velocity
 */
Vector2D* PathPlanner::GetNavigationVectorToWorksite() {
	if (relVectorToWorksite != NULL) {
		return this->GetNavigationVectorTo(relVectorToWorksite);
	}
	return (new Vector2D(0,0));
}

/**
 * Returns a vector by which wheels should move in order to navigate to a certain vector
 * that is expressed in robot's LOCAL coordinates
 * X is left wheel velocity
 * Y is right wheel velocity
 */
Vector2D* PathPlanner::GetNavigationVectorTo(Vector2D* targetVec_) {

	double angleToTargetInDeg = (targetVec_->x * 180/PI);
	Vector2D* returnVec = new Vector2D();

	if (DEBUG_NAVIGATION_TO_TARGET) {
		for (int i=0; i<8; i++) {
			epuckBoardController->SetLED(i,0);
		}
	}
	if (DEBUG_NAVIGATION_TO_TARGET == 1) { std::cout << "Navigating to target with rel vec " << targetVec_->GetOutputString() << " angle: " << angleToTargetInDeg; }

	//-- if target in front, pick a random choice of how it will turn towards a target at the back next time
	if (angleToTargetInDeg < 20 && targetVec_->y > 0 ) {
		rotateTowardsTargetBehindByLeft = (mathHelpers_generateRand(0,1) > 0.5);
		//std::cout << "R" << robot->thisRobotId << " next time turning back to left:" << rotateTowardsTargetBehindByLeft << std::endl;
	} else if (angleToTargetChangeCounter > 100) {
		angleToTargetChangeCounter = 0;
		rotateTowardsTargetBehindByLeft = (mathHelpers_generateRand(0,1) > 0.5);
	} else {
		angleToTargetChangeCounter++;
	}


	if (targetVec_->y < 0 || angleToTargetInDeg > 150 ) {
		if (DEBUG_NAVIGATION_TO_TARGET == 1) {
			std::cout << "  TARGET BEHIND ";
			epuckBoardController->SetLED(3,1);
			epuckBoardController->SetLED(4,1);
			epuckBoardController->SetLED(5,1);
		}
		//-- behind robot, rotate a lot
		if (rotateTowardsTargetBehindByLeft) {
			returnVec->x = 0 *ROBOT_MAX_SPEED;
			returnVec->y = 0.5* ROBOT_MAX_SPEED;
		} else {
			returnVec->x = 0.5*ROBOT_MAX_SPEED;
			returnVec->y = 0* ROBOT_MAX_SPEED;
		}
	} else if (fabs(angleToTargetInDeg) < 10) {
		if (DEBUG_NAVIGATION_TO_TARGET == 1) {
			std::cout << " target in front ";
			epuckBoardController->SetLED(7,1);
			epuckBoardController->SetLED(0,1);
			epuckBoardController->SetLED(1,1);
		}
		returnVec->x = ROBOT_MAX_SPEED;
		returnVec->y = ROBOT_MAX_SPEED;

	} else if (targetVec_->x > 0){
		if (DEBUG_NAVIGATION_TO_TARGET == 1) {
			std::cout << " target on right ";
			epuckBoardController->SetLED(1,1);
			epuckBoardController->SetLED(2,1);
			epuckBoardController->SetLED(3,1);
		}

		if (fabs(angleToTargetInDeg) < 20) {
			if (DEBUG_NAVIGATION_TO_TARGET == 1) { std::cout << " (soft) "; }
			//-- soft turncurrentGlobalPosition
			returnVec->x = 0.5*ROBOT_MAX_SPEED;
			returnVec->y = 0.25*ROBOT_MAX_SPEED;
		} else {
			//-- sharp turn
			returnVec->x = 0.5*ROBOT_MAX_SPEED;
			returnVec->y = 0*ROBOT_MAX_SPEED;
		}

	} else {
		if (DEBUG_NAVIGATION_TO_TARGET == 1) {
			std::cout << " target on left ";
			epuckBoardController->SetLED(5,1);
			epuckBoardController->SetLED(6,1);
			epuckBoardController->SetLED(7,1);
		}
		if (fabs(angleToTargetInDeg) < 20) {
			if (DEBUG_NAVIGATION_TO_TARGET == 1) { std::cout << " (soft) "; }
			//-- soft turn
			returnVec->x = 0.25*ROBOT_MAX_SPEED;
			returnVec->y = 0.5*ROBOT_MAX_SPEED;
		} else {
			//-- sharp turn
			returnVec->x = 0*ROBOT_MAX_SPEED;
			returnVec->y = 0.5*ROBOT_MAX_SPEED;
		}

	}

	if (DEBUG_NAVIGATION_TO_TARGET == 1) { std::cout << std::endl; }
	return returnVec;
}

/**
 * Returns a vector by which wheels should move in order to navigate to base, where
 * X is left wheel velocity
 * Y is right wheel velocity
 */
Vector2D* PathPlanner::GetNavigationVectorAwayFromBase() {

	double angleToBaseInDeg = (relVectorToBase->x * 180/PI);
	Vector2D* returnVec = new Vector2D();
	if (DEBUG_NAVIGATION_FROM_BASE == 1) { std::cout << "Navigating to base angle " << angleToBaseInDeg << "y" << relVectorToBase->y; }
	if (relVectorToBase->y > 0) {
		//-- facing base, rotate a lot
		if (DEBUG_NAVIGATION_FROM_BASE == 1) { std::cout << "  BASE IN FRONT "; }
		returnVec->x = -ROBOT_MAX_SPEED;
		returnVec->y = ROBOT_MAX_SPEED;
	} else if (fabs(angleToBaseInDeg) < 20 && relVectorToBase->y < 0 ) {
		if (DEBUG_NAVIGATION_FROM_BASE == 1) { std::cout << " base behind "; }
		returnVec->x = 0.5*ROBOT_MAX_SPEED;
		returnVec->y = 0.5*ROBOT_MAX_SPEED;
	} else if (relVectorToBase->x > 0){
		if (DEBUG_NAVIGATION_FROM_BASE == 1) { std::cout << " base on right "; }
		returnVec->x = -0.25*ROBOT_MAX_SPEED;
		returnVec->y = 0.25*ROBOT_MAX_SPEED;
	} else {
		if (DEBUG_NAVIGATION_FROM_BASE == 1) { std::cout << " base on left "; }
		returnVec->x = 0.25*ROBOT_MAX_SPEED;
		returnVec->y = +0.25*ROBOT_MAX_SPEED;
	}

	if (DEBUG_NAVIGATION_FROM_BASE == 1) { std::cout << std::endl; }
	return returnVec;
}


/**
 * Returns true if the Robot is within the Nest
 */
bool PathPlanner::GetIsRobotInsideBase(){
  return (this->GetDistanceToBase() <= robot->baseRadius);
}


/**
 * Returns true if the Robot is 10 cm away from the arena boundaries
 */
bool PathPlanner::GetIsRobotOutOfBoundaries(){
  //std::cout << " PathPlanner::GetIsRobotOutOfBoundaries: X=" << xPos << " Y=" << yPos << std:: endl;
  return (fabs(currentGlobalPosition->x) >= 8500 || currentGlobalPosition->y >= 7200 || currentGlobalPosition->y < -6600);
}

/* ===================================== LOWER LEVEL VECTOR STUFF ========================*/

/*
 * Get vector to an object translated into local coordinates
 * @param vector_ CRVector3d vector to point towards
 * @return CRVector3d vector where X component represents angle towards the object and y component distance
 */
Vector2D* PathPlanner::ConvertVectorToLocalCoordinateSystem(Vector2D* vector_, bool isNavigationToTarget_) {
	double rot = -(double)IPCROSVICON->GetRobotPositionA();

	Vector2D* tempVec = new Vector2D();
	tempVec->x = vector_->x*cos(rot) + vector_->y*sin(rot);
	tempVec->y = -vector_->x*sin(rot) + vector_->y*cos(rot);

	//if (DEBUG_NAVIGATION_TO_TARGET == 1 && isNavigationToTarget_) { std::cout << " CALC2 rot : " << rot*180/PI << " VEC: " << vector_->GetOutputString() << " local coord syst " << tempVec->GetOutputString() ; }
	return tempVec;
}

/**
 * Returns relative vector to something in polar coordinates, where X is degrees (positive is to the robot's right)
 * and Y is distance (positive is in front, negative behind) (TODO!)
 */
Vector2D* PathPlanner::CalculateRelativeVectorTo(Vector2D* vector_) {
	return CalculateRelativeVectorTo(vector_, false);
}
Vector2D* PathPlanner::CalculateRelativeVectorTo(Vector2D* vector_, bool isNavigationToTarget_) {
	Vector2D* tempVector = new Vector2D();
	tempVector->x =  vector_->x - currentGlobalPosition->x;
	tempVector->y =  vector_->y - currentGlobalPosition->y;

	Vector2D* u = this->ConvertVectorToLocalCoordinateSystem(tempVector, isNavigationToTarget_);
	u->Normalize();
	if (isnan(u->x)) {
		u->x = 0;
	}
	//if (DEBUG_NAVIGATION_TO_TARGET ==  1 && isNavigationToTarget_) { std::cout << "  x in deg" << (u->x * 180 / PI); }


	//-- calc distance with direction
	double distance = sqrt(pow(tempVector->x,2) + pow(tempVector->y,2));

	//add direction:
	if (u->y < 0) {
		distance = -distance;
	}
	u->y = distance;

	//if (DEBUG_NAVIGATION_TO_TARGET == 1 && isNavigationToTarget_) { std::cout << " Y with distance " << u->y << std::endl; }
	return u;
}


/* =========================== GETTERS / SETTERS ========================= */
double PathPlanner::GetDistanceToBase() { return fabs(relVectorToBase->y); }

double PathPlanner::GetDistanceToWorksite() {
	if (relVectorToWorksite != NULL) {
		return fabs(relVectorToWorksite->y);
	}
	return -1;
}

void PathPlanner::SetRelVectorToWorksiteFromGlobalCoords(double globalX_, double globalY_) {
	Vector2D* globalWorksiteCord = new Vector2D(globalX_, globalY_);
	relVectorToWorksite = this->CalculateRelativeVectorTo(globalWorksiteCord);

	delete globalWorksiteCord;
}

/**
 * Used for some debugging, not really reliable
 */
bool PathPlanner::GetHasValidData() {
	return !( IPCROSVICON->GetRobotPositionX() == 0 && IPCROSVICON->GetRobotPositionY() == 0 );
}



