//File created by Radhika (July 2011)
//Implementation of SharedControl class

#include "StdAfx.h"
#include "SharedControl.h"

//#define MODE_WALL_FOLLOW

SharedControl::SharedControl(CWheelchairApp* mainApp)
{
	m_pMainApp = mainApp;
	m_pStateControl = m_pMainApp->m_pStateControl;
	
	correctionCount = 0;	
	wallSide = 1; //default wall is left
	modeWallFollow = false;
	threshold = 0.0;
	sideThreshold = 0.0;
	wallDistance = 5.0; //high value so as not to detect
	proximityThreshold = 1.0; //high value so as not to move close to a wall

	for (int i=0; i<3; i++){
		velocity[i] = 0;
	}

	for (int i=0; i<3; i++){
		curIRSensor[i] = 0;
		prevIRSensor[i] = 0;
	}

	for (int i=0; i<9; i++){
		sonarSensor[i] = 5.0;
	}

	for (int i=0; i<5; i++){
		obstacleDir[i] = 0;
	}

}

SharedControl::~SharedControl(void)
{
	//TODO
}

void SharedControl::SetSensorRange(CString msg)
{
	CString str, msg_content;
	str = msg;
	int start = 4; //String begins with "SEN ". Hence start with "{" which is at position 4 of the string.
	
	//TRACE0("Entered SetSensorRange\n");

	//TODO: Modify if expecting String to contain "{Time}" data before "{Type}"
	
	msg_content = "";
	
	do{
		//TODO: Add following in try-catch
		msg_content = str.Tokenize(" ",start); //Expecting "{Type "
		msg_content.Delete(0,1); //Remove the "{"
	}while((msg_content!="Type") || (msg_content == "\r"));
	
	if (msg_content == "Type"){
		msg_content = str.Tokenize("}", start);
		
		if(msg_content == "GroundTruth"){
			do{
				//TODO: Add following in try-catch
				msg_content = str.Tokenize(" ",start); //Expecting "{Velocity "
				msg_content.Delete(0,1); //Remove the "{"
			}while((msg_content!="Velocity") || (msg_content == "\r"));

			if(msg_content == "Velocity"){
				for(int i = 0; i < 2; i++){
					msg_content = str.Tokenize(",", start);
					velocity[i] = atof(msg_content);
					//TRACE1("Velocity = (%f, ", velocity[i]);
				}
				msg_content = str.Tokenize("}", start);
				velocity[2] = atof(msg_content);
				//TRACE1("%f)", velocity[2]);
			}
		}else if (msg_content == "Sonar"){
			TRACE0("Detected Sound Sensor\n");
			for (int i = 0; i < 9; i++) {
				msg_content = str.Tokenize("{", start);
				msg_content.Delete(0,2);
				msg_content = str.Tokenize(" ", start);
				if (msg_content == "Name"){
					msg_content = str.Tokenize(" ", start);
					//TRACE1("\n%s ", msg_content.GetString());
					msg_content = str.Tokenize(" ", start); //Remove "Range "
					if (msg_content == "Range"){
						msg_content = str.Tokenize("}", start); //Get the range value
						sonarSensor[i] = atof(msg_content); //TODO get rid of warning for double to float conversion
						//TRACE1("= %f\n", sonarSensor[i]);
					} else {
						TRACE1("Range not specified for Sonar %d Sensor. Go to next sensor!", i);
						continue;
					}
				} else {
					TRACE1("Name not specified for Sonar %d Sensor. Go to next sensor!", i); 
					continue;
				}
			}//for i<10
						
		//end of Sonar

		}else if (msg_content == "IR"){
			//TRACE0("Detected IR\n");
		
			//Here it is assumed that the sensor range data is displayed in the the following format:
			//SEN {Type IR} {Name IRRight Range 1.6234} {Name IRLeft Range 2.0000} {Name IRFront Range 1.2345}
			
			for (int i = 0; i < 4; i++) {
				msg_content = str.Tokenize("{", start);
				msg_content.Delete(0,2);
				msg_content = str.Tokenize(" ", start);
				if (msg_content == "Name"){
					msg_content = str.Tokenize(" ", start);
					//TRACE1("\n%s ", msg_content.GetString());
					msg_content = str.Tokenize(" ", start); //Remove "Range "
					if (msg_content == "Range"){
						msg_content = str.Tokenize("}", start); //Get the range value
						prevIRSensor[i] = curIRSensor[i];
						curIRSensor[i] = atof(msg_content); //TODO get rid of warning for double to float conversion
						//TRACE1("= %f\n", curIRSensor[i]);
					} else {
						TRACE1("Range not specified for IR %d Sensor. Go to next sensor!", i);
						continue;
					}
				} else {
					TRACE1("Name not specified for IR %d Sensor. Go to next sensor!", i); 
					continue;
				}
			} //for i<4
			
			SetMode();

		} //IR
		
		}//msg_content = "Type" 

	//TRACE0("Exited SetSensorRange\n");
}

void SharedControl::SetMode(void)
{
			CheckObstacleDir();
			AvoidCollision();		
			modeWallFollow = false;

#ifdef MODE_WALL_FOLLOW
			DetectWall();
#endif
			AvoidObstacle();

#ifdef MODE_WALL_FOLLOW
			if(modeWallFollow == true){
				AutoWallFollow();
			}
#endif
}

void SharedControl::CheckObstacleDir(void)
{
	if (VelocityLevel() == VELOCITY_0){
		threshold = 1.2;
	}else {
		threshold = 2 * (0.6 +((VelocityLevel() - 1) * 0.4));
	}

	sideThreshold = (threshold/2);
	//TRACE2("Threshold = %f, SideThreshold = %f\n", threshold, sideThreshold);

	ResetObstacleDir();

	if(sonarSensor[0] <= sideThreshold){
		obstacleDir[RIGHT_SIDE] = 1;
	}
	if((sonarSensor[1]<= threshold) || (sonarSensor[2]<= threshold)){
		obstacleDir[RIGHT_CORNER] = 1;
	}	
	if((sonarSensor[3] <= threshold) || (sonarSensor[4] <= threshold) || (sonarSensor[5] <= threshold)){
		obstacleDir[FRONT] = 1;
	}
	if((sonarSensor[6] <= threshold) || (sonarSensor[7] <= threshold)){
		obstacleDir[LEFT_CORNER] = 1;
	}	
	if(sonarSensor[8] <= sideThreshold){
		obstacleDir[LEFT_SIDE] = 1;
	}		

	/*for (int i=0; i<5; i++){
		TRACE2("ObstacleDir[%d] = %d\n", i, obstacleDir[i]);
	}*/
	
}

void SharedControl::AvoidObstacle(void)
{
	TRACE0("\nAvoid Obstacle\n");
	
	int userState = STOP;
	bool safe = false;

	state = m_pStateControl->GetState();
	userState = m_pStateControl->GetUserRequest();
	TRACE2("State =%d, UserState =%d\n", state, userState);


	if ((state == STOP)){
		modeWallFollow = false;
		correctionCount = 0;
		userState = STOP;
		return;
	}

	//in states RIGHT, LEFT or FORWARD...proceed:

#ifdef MODE_WALL_FOLLOW
	safe = NavigateCorner();
	if (safe == true){
		TRACE0("Corner Navigated for Wall Follow mode!\n");
		modeWallFollow = false;
		return;
	}
#endif

	safe = FrontBigObstacle();
	if (safe == true){
		TRACE0("Front protected from big obstacle!\n");
		modeWallFollow = false;
		return;
	}

#ifdef MODE_WALL_FOLLOW
	if(modeWallFollow == true){
		return;
	}
#endif

	safe = FrontSmallObstacle();
	if (safe == true){
		TRACE0("Front protected from small obstacle!\n");
		modeWallFollow = false;
		return;
	}

	safe = CornerObstacle();
	if (safe == true){
		TRACE0("Corner protected!\n");
		return;
	}	

	if(correctionCount == 1){
		TRACE0("correction has been made");
		
		//if wheelchair's direction is not as requested by user, change the direction
		if(userState != state){
			TRACE0("wheelchair's direction is not as requested by user\n");
			correctionCount = 0;
			m_pStateControl->SharedTransFun(userState,VelocityLevel());
		}
	}
			
}

bool SharedControl::NavigateCorner(void)
{
	double IRThreshold = 0.8; //an extra allowance of 0.15m is provided over 0.65m max distance threshold

	if((sonarSensor[2] > threshold) || (sonarSensor[3] > threshold) 
		|| (sonarSensor[4] > threshold) || (sonarSensor[5] > threshold) || (sonarSensor[6] > threshold)
		|| (sonarSensor[1] > (threshold + (0.2*VelocityLevel()))) || (sonarSensor[7] > (threshold + (0.2*VelocityLevel())))){
		TRACE0("Front not obstructed!\n");
		return false;
	}

	if(modeWallFollow == true){
		if((curIRSensor[0] <= IRThreshold) && (curIRSensor[1] <= IRThreshold)){
			//wall on either side, cannot turn
			TRACE0("Wall on either side!\n");
			return false;
		}else {
			if(wallSide == 0){
				if(obstacleDir[LEFT_SIDE] == 1){
					TRACE0("Obstructed in left!\n");
					return false;
				}else{
					TRACE0("Right wall, turning left!\n");
					m_pStateControl->SharedTransFun(4,(VelocityLevel() + 1)); //left
					m_pStateControl->SharedTransFun(4,(VelocityLevel() + 1)); //left
					correctionCount =0;
					return true;
				}
			}else if (wallSide == 1){
				if(obstacleDir[RIGHT_SIDE] == 1){
					TRACE0("Obstructed in right!\n");
					return false;
				}else{
					TRACE0("Left wall, turning right!\n");
					m_pStateControl->SharedTransFun(2,(VelocityLevel() + 1)); //left
					m_pStateControl->SharedTransFun(2,(VelocityLevel() + 1)); //left
					correctionCount =0;
					return true;
				}
			}
		}
	}

		TRACE0("NavigateCorner returning false!\n");
		return false;
		
}

bool SharedControl::FrontBigObstacle(void)
{
	if(obstacleDir[FRONT] == 0){
		TRACE0("Front not obstructed!\n");
		return false;
	}

	//if whole front is obstructed
	if((sonarSensor[3] <= threshold) && (sonarSensor[4] <= threshold) && (sonarSensor[5] <= threshold) && (sonarSensor[2] <= threshold) && (sonarSensor[6] <= threshold)){
		TRACE0("Front completely blocked!\n");
		//if corners also blocked
		if((sonarSensor[1] <= threshold) && (sonarSensor[7] <= threshold)){
			TRACE0("Corners also blocked\n");
			m_pStateControl->SharedTransFun(0,VelocityLevel()); //stop
			correctionCount =1;
			return true;
		}
	}
				
	TRACE0("FrontBigObstacle returning false!\n");
	return false;
}

bool SharedControl::FrontSmallObstacle(void)
{
	if(obstacleDir[FRONT] == 0){
		TRACE0("Front not obstructed!\n");
		return false;
	}

	//if left front is obstructed
	if (((sonarSensor[4] <= threshold) ||(sonarSensor[5] <= threshold)) && (sonarSensor[3] > threshold)){
		TRACE0("Left Front blocked!\n");
		if(obstacleDir[RIGHT_CORNER] == 0){
			TRACE0("Right corner free. turn right!\n");
			m_pStateControl->SharedTransFun(2,(2*VelocityLevel()+1)); //turn right
			correctionCount =1;
			return true;
		}else{
			TRACE0("Right corner not free. stop!\n");
			m_pStateControl->SharedTransFun(0,VelocityLevel()); //stop
			correctionCount =1;
			return true;
		}
	}

	//if right front is obstructed
	else if (((sonarSensor[3] <= threshold) || (sonarSensor[4] <= threshold)) && (sonarSensor[5] > threshold)){
		TRACE0("Right Front blocked!\n");
		if(obstacleDir[LEFT_CORNER] == 0){
			TRACE0("Left corner free. turn left!\n");
			m_pStateControl->SharedTransFun(4,(2*VelocityLevel()+1)); //turn left
			correctionCount =1;
			return true;
		}else{
			TRACE0("Left corner not free. stop!\n");
			m_pStateControl->SharedTransFun(0,VelocityLevel()); //stop
			correctionCount =1;
			return true;
		}
	}

	//if only small object up front, turn right
	else if((sonarSensor[4] <= threshold) && (sonarSensor[3] > threshold) && (sonarSensor[5] > threshold)){
		if(obstacleDir[RIGHT_CORNER] == 0){
			TRACE0("Right corner free. turn right!\n");
			m_pStateControl->SharedTransFun(2,(VelocityLevel() +1)); //turn right
			correctionCount =1;
			return true;
		}else if(obstacleDir[LEFT_CORNER] == 0){
			TRACE0("Left corner free. turn left!\n");
			m_pStateControl->SharedTransFun(4,(VelocityLevel()+1)); //turn left
			correctionCount =1;
			return true;
		}else{
			TRACE0("Corners blocked. stop!\n");
			m_pStateControl->SharedTransFun(0,VelocityLevel()); //stop
			correctionCount =1;
			return true;
		}
	}

	//TODO - missing routine for handling obstacle detected by all 5 front sensors

	TRACE0("FrontSmallObstacle returning false!\n");
	return false;	

}

bool SharedControl::CornerObstacle(void)
{
	int corner = 0;
	int sharpTurn = 1;

	if((obstacleDir[RIGHT_CORNER] == 0) && (obstacleDir[LEFT_CORNER] == 0)){
		TRACE0("Corners not obstructed\n");
		return false;
	}else if ((obstacleDir[RIGHT_CORNER] == 1) && (obstacleDir[LEFT_CORNER] == 1)){
		TRACE0("Both corners obstructed\n");
		//check which obstacle is closer & handle that side first
		if ((sonarSensor[7] <= sonarSensor[1]) || (sonarSensor[6] <= sonarSensor[2])){
			corner = 1; //left corner
		}else{
			corner = 2; //right corner
		}
	}else if((obstacleDir[RIGHT_CORNER] == 1) || (obstacleDir[LEFT_CORNER] == 1)){
		TRACE0("Only one corner obstructed\n");
		corner = 1;
	}

	if((obstacleDir[FRONT] == 1)){
		TRACE0("Will take sharp turns!\n");
		sharpTurn = 2;
	}
	
	if((corner == 1) && (modeWallFollow == true)){
		if(((wallSide == 0) && (obstacleDir[RIGHT_CORNER] == 1)) || ((wallSide == 1) && (obstacleDir[LEFT_CORNER] == 1))){
			TRACE0("Corner on same side as wall\n");
			return false;
		}
	}

	switch (corner){
		case 1:
			if(obstacleDir[LEFT_CORNER] == 1){
				TRACE0("Left corner\n");
				if(obstacleDir[RIGHT_SIDE] == 0){
					TRACE0("Right turn!\n");
					m_pStateControl->SharedTransFun(2,(sharpTurn*VelocityLevel())); //turn right
					correctionCount =1;
					return true;
				}else{
					TRACE0("Right not clear.Stop\n");
					m_pStateControl->SharedTransFun(0,VelocityLevel());//stop
					correctionCount =1;
					return true;
				}
			}
		
		case 2:
			if(obstacleDir[RIGHT_CORNER] == 1){		
				TRACE0("Right corner\n");
				if(obstacleDir[LEFT_SIDE] == 0){
					TRACE0("Left turn\n");
					m_pStateControl->SharedTransFun(4,(sharpTurn*VelocityLevel())); //turn left
					correctionCount =1;
					return true;
				}else{
					TRACE0("Left not clear. Stop!\n");
					m_pStateControl->SharedTransFun(0,VelocityLevel());//stop
					correctionCount =1;
					return true;
				}
			}
			break;
	}

	TRACE0("ProtectCorner returning false!\n");
	return false;
}

void SharedControl::ResetObstacleDir(void)
{
	for(int i = 0; i < 5; i++){
		obstacleDir[i] = 0;
	}	
}

//Assumptions for Wall Following mode:
//The user guides the wheelchair to position it on the right or left of a wall
// Walls to the front and back are not currently handled
void SharedControl::DetectWall(void)
{	
	wallDistance = 1.00; //distance to wall at which wall will be detected
	proximityThreshold = ((0.1 * VelocityLevel()) + 0.1); //closeness to the wall
	
	//TRACE1("WallFollow mode status = %d\n", modeWallFollow);

	//value 0.6 experimentally determined
	if (((curIRSensor[0] <= wallDistance) && (prevIRSensor[0] < wallDistance) && (curIRSensor[2] <= wallDistance + 0.6)) || 
		((curIRSensor[1] <= wallDistance) && (prevIRSensor[1] < wallDistance) && (curIRSensor[3] <= wallDistance + 0.6))){
			
			if((curIRSensor[0] <= wallDistance) && (curIRSensor[1] <= wallDistance)){
				if((curIRSensor[0] <= (proximityThreshold + 0.2)) && (curIRSensor[1] <= (proximityThreshold + 0.2))){
					modeWallFollow = false;
				}else {

					if (curIRSensor[1] <= curIRSensor[0]){
						wallSide = 1;
					}else{
						wallSide = 0;
					}
				modeWallFollow = true;
				}
		}else if (curIRSensor[0] <= wallDistance){
			wallSide = 0;
			modeWallFollow = true;
		}else if (curIRSensor[1] <= wallDistance){
			wallSide = 1;
			modeWallFollow = true;
		}else{
			modeWallFollow = false;
		}	
	}else{
		modeWallFollow = false;
	}

	TRACE2("DetectWall: modeWallFollow = %d, wallSide = %d\n", modeWallFollow, wallSide);

	if (modeWallFollow == true){
		correctionCount =0;
	}
}

//Strategy: User must bring the wheelchair close to the wall (> 0.2m and <0.5m)
//Shared control automatically follows wall once in this zone
//If the user repeatedly presses the key corresponding to the opposite direction as that of the wall
// we exit the 'Wall Following' mode
void SharedControl::AutoWallFollow(void)
{
	double proximityThreshold = ((0.1 * VelocityLevel()) + 0.1); //closeness to the wall
	double distanceThreshold = 0.65; //don't allow wheelchair to go further away than an arm's distance from the wall
		
	double angleHigh = 0.8988; //angle in radians corresponding to 51.5 degrees away from normal to wall
	double angleLow = 0.8464; //angle in radians corresponding to 48.5 degrees away from normal to wall

	state = m_pStateControl->GetState();

	if((state == STOP) || (state == BACKWARD)){
		modeWallFollow = false;
		return;
	}

	TRACE2("AutoWallFollow for wall %d when in state %d\n", wallSide, state);
	//TRACE2("Threshold = %f for Vel Level %d \n", proximityThreshold, VelocityLevel());

	switch (wallSide){

	case 0:
			switch (state){
				case LEFT:
					
					if((curIRSensor[0] < (proximityThreshold)) || (curIRSensor[2] < (proximityThreshold))){
						//TRACE0("Will hit! Turn left!\n");
						//SharedTransFun(4, VelocityLevel());
						break;
					}
					//if (wheelchair directed towards wall){ dont do anything, allow wheelchair to move left}
					if(curIRSensor[0] > curIRSensor[2]){ 
						break; 
					}

					if((curIRSensor[0] > prevIRSensor[0]) && curIRSensor[0] >= distanceThreshold){
						//TRACE0("Prevent exit from wall follow!\n");
						m_pStateControl->SharedTransFun(2, VelocityLevel());
						break;
					}

					if((curIRSensor[0] <= curIRSensor[2]) // wheelchair away from wall
						&& (acos(curIRSensor[0]/curIRSensor[2])) > angleHigh){ //and if angle > 51.5 degrees (using (cos a = adjacent/hypotenuse), 
						if((curIRSensor[0] >= 0.3) && (curIRSensor[2] >= 0.3)){ //as long as you are not too close to the wall (theshold 30cm)
						m_pStateControl->SharedTransFun(2, VelocityLevel()); //turn right towards the wall
						//TRACE0("Turn right!\n");
						}
					}else {
						m_pStateControl->SharedTransFun(3, VelocityLevel()); //wheelchair is towards wall and either angle is between 50 and 52.5 degrees or below threshold 30 cm
						//TRACE0("Go straight!\n");
					}
					break; //end of LEFT
				
				case FORWARD:	

					if((curIRSensor[0] < proximityThreshold) || (curIRSensor[2] < proximityThreshold)){
					//TRACE0("Will hit! Turn left!\n");
					m_pStateControl->SharedTransFun(4, (VelocityLevel()));
					break;
					}	

					if((curIRSensor[0] > prevIRSensor[0]) && curIRSensor[0] >= distanceThreshold){
						//TRACE0("Prevent exit from wall follow!\n");
						m_pStateControl->SharedTransFun(2, VelocityLevel());
						break;
					}

					if(((curIRSensor[0] > curIRSensor[2]) && (curIRSensor[0] <= (proximityThreshold + 0.3))) || ((curIRSensor[0] <= curIRSensor[2]) && (curIRSensor[0] <= 0.4) && ((acos(curIRSensor[0]/curIRSensor[2])) < angleLow))){
						//TRACE0("Turn left!\n");
						m_pStateControl->SharedTransFun(4, VelocityLevel());
					}else if ((curIRSensor[0] <= curIRSensor[2]) && ((acos(curIRSensor[0]/curIRSensor[2])) > angleHigh)){
						//TRACE0("Turn right!\n");
						m_pStateControl->SharedTransFun(2, VelocityLevel());
					}
					break; //end of FORWARD

				case RIGHT:	
		
					if((curIRSensor[0] < proximityThreshold) || (curIRSensor[2] < proximityThreshold)){
					//TRACE0("Will hit! Turn left!\n");
					m_pStateControl->SharedTransFun(4, (VelocityLevel()));
					break;
					}						
					if((curIRSensor[0] > curIRSensor[2]) || ((curIRSensor[0] <= curIRSensor[2]) && (curIRSensor[0] <= 0.4)&& ((acos(curIRSensor[0]/curIRSensor[2])) < angleLow))){
						//TRACE0("Turn left!\n");
						m_pStateControl->SharedTransFun(4, VelocityLevel());
					}
					break; //end of RIGHT
			}//end of switch for state
			break; //for case 0

			case 1:
				TRACE0("\nIn case for left wall\n");

			switch (state){

				case RIGHT:
					if((curIRSensor[1] < (proximityThreshold)) || (curIRSensor[3] < (proximityThreshold))){
						//TRACE0("Will hit! Turn right!\n");
						//SharedTransFun(2, VelocityLevel());
						break;
					}
					//if (wheelchair directed towards wall){ dont do anything, allow wheelchair to move right}
					if(curIRSensor[1] > curIRSensor[3]){ 
						break; 
					}

					if((curIRSensor[1] > prevIRSensor[1]) && curIRSensor[1] >= distanceThreshold){
						TRACE0("Prevent exit from wall follow!\n");
						m_pStateControl->SharedTransFun(4, VelocityLevel());
						break;
					}

					if((curIRSensor[1] <= curIRSensor[3]) // wheelchair away from wall
						&& (acos(curIRSensor[1]/curIRSensor[3])) > angleHigh){ //and if angle > 51.5 degrees (using (cos a = adjacent/hypotenuse), 
						if((curIRSensor[1] >= 0.3) && (curIRSensor[3] >= 0.3)){ //as long as you are not too close to the wall (theshold 30cm)
						m_pStateControl->SharedTransFun(4, VelocityLevel()); //turn left towards the wall
						TRACE0("Turn left!\n");
						}
					}else {
						m_pStateControl->SharedTransFun(3, VelocityLevel()); //wheelchair is towards wall and either angle is between 50 and 52.5 degrees or below threshold 30 cm
						TRACE0("Go straight!\n");
					}
					break;//end of RIGHT
				
				case FORWARD:	
					
					if((curIRSensor[1] < proximityThreshold) || (curIRSensor[3] < proximityThreshold)){
					TRACE0("Will hit! Turn right!\n");
					m_pStateControl->SharedTransFun(2, (VelocityLevel()));
					break;
					}	

					if((curIRSensor[1] > prevIRSensor[1]) && curIRSensor[1] >= distanceThreshold){
						TRACE0("Prevent exit from wall follow!\n");
						m_pStateControl->SharedTransFun(4, VelocityLevel());
						break;
					}

					if(((curIRSensor[1] > curIRSensor[3]) && (curIRSensor[1] <= (proximityThreshold + 0.3))) || ((curIRSensor[1] <= curIRSensor[3]) && (curIRSensor[1] <= 0.4) && ((acos(curIRSensor[1]/curIRSensor[3])) < angleLow))){
						TRACE0("Turn right!\n");
						m_pStateControl->SharedTransFun(2, VelocityLevel());
					}else if ((curIRSensor[1] <= curIRSensor[3]) && ((acos(curIRSensor[1]/curIRSensor[3])) > angleHigh)){
						TRACE0("Turn left!\n");
						m_pStateControl->SharedTransFun(4, VelocityLevel());
					}
					break;//end of FORWARD

				case LEFT:	
					
					if((curIRSensor[1] < proximityThreshold) || (curIRSensor[3] < proximityThreshold)){
					TRACE0("Will hit! Turn right!\n");
					m_pStateControl->SharedTransFun(2, (VelocityLevel()));
					break;
					}						
					if((curIRSensor[1] > curIRSensor[3]) || ((curIRSensor[1] <= curIRSensor[3]) && (curIRSensor[1] <= 0.4)&& ((acos(curIRSensor[1]/curIRSensor[3])) < angleLow))){
						TRACE0("Turn right!\n");
						m_pStateControl->SharedTransFun(2, VelocityLevel());
					}
					break; //end of LEFT
			}//end of switch for state
			break; //for case 1

		}//end of switch for cond
}

void SharedControl::AvoidCollision(void)
{
	//TRACE0("In fn: Collision avoidance\n");
	
	double colThreshold = threshold/2;
	int colRisk[5];

	for(int i = 0; i < 5; i++){
		colRisk[i] = 0;
	}

	if(sonarSensor[0] <= (colThreshold/2)){
		colRisk[RIGHT_SIDE] = 1;
	}
	if((sonarSensor[1]<= colThreshold)){
		colRisk[RIGHT_CORNER] = 1;
	}	
	if((sonarSensor[3] <= colThreshold) || (sonarSensor[4] <= colThreshold) || (sonarSensor[5] <= colThreshold) || (sonarSensor[6] <= colThreshold) ||(sonarSensor[2]<= colThreshold)){
		colRisk[FRONT] = 1;
	}
	if((sonarSensor[7] <= colThreshold)){
		colRisk[LEFT_CORNER] = 1;
	}	
	if(sonarSensor[8] <= (colThreshold/2)){
		colRisk[LEFT_SIDE] = 1;
	}	

	m_pStateControl->SetCollisionRisk(colRisk);

}

//Empirically found the velocity to fall into 4 levels - roughly 0.4m/s, 0.8m/s, 1.2m/s, 1.7m/s
int SharedControl::VelocityLevel()
{
	if ((abs(velocity[0]) >= 1.3) || (abs(velocity[1]) >= 1.3)){	
		return VELOCITY_4;
	}else if((abs(velocity[0]) >= 0.9) || (abs(velocity[1]) >= 0.9)){
		return VELOCITY_3;
	}else if((abs(velocity[0]) >= 0.5) || (abs(velocity[1]) >= 0.5)){
		return VELOCITY_2;
	}else if((abs(velocity[0]) >= 0.2) || (abs(velocity[1]) >= 0.2)){
		return VELOCITY_1;
	}else {
		return VELOCITY_0;
	}
}