#include "StdAfx.h"
#include "StateControl.h"
#include <time.h>

/* July 2011
Modifications made by Radhika for implementation of Shared Control 
are explicity mentioned with comments labelled "start and end radhika"
*/

void * StateControl::pObject;	//!< Needed in TimerProc_Wrapper

StateControl::StateControl(CWheelchairApp* mainApp)
{
	m_pMainApp = mainApp;

	state = STOP;
	right = 0;
	left = 0;
	last_time = 0;
	timer = NULL;
	pObject = this;		// Needed for timer wrapper


	//radhika start
	for (int i=0; i<5; i++){
		collisionRisk[i] = 0;
	}

	userRequest = STOP;
	//end radhika

}

void StateControl::TransFun(int cond) {
	TRACE2("IN: %d %d",cond, state);
// First, calculate the new state:

	//radhika start
	if(m_pMainApp->m_useSharedControl){
		TRACE0("TransFun, using Shared ctrl\n");
		bool proceed;
		proceed = CheckCollision(cond);
		TRACE1("Collision checked. Proceed = %d\n", proceed);
		if (proceed == true){
			proceed = ModifyControl(cond);
		}
		TRACE1("Control modified = %d\n", proceed);
		if(proceed == false){
			return;
		}
	}
	//end radhika

	switch (cond) {
		case FORWARD:
			switch (state) {
				case FORWARD:
				case STOP:
				case BACKWARD:
					// do nothing if already at max speed
					//radhika start
					if (right == 50 && left == 50) //changed 100 to 50 for testing
					//end radhika
						break;
					// otherwise increase forward speed
					right += 25;
					left += 25;
					// update the state
					if (right < 0)
						state = BACKWARD;
					else if (right == 0)
						state = STOP;
					else
						state = FORWARD;
					break;
				case LEFT:
				case RIGHT:
					right += (left - right)/2;
					if (right == 90)
						right = 100;
					if (right == 80)
						right = 75;
					left  = right;
					if (right <= 0 || left <= 0)
						left = right = 25;
					state = FORWARD;
					//! \todo change this to go through STOP.
			}
			break;
		case BACKWARD:
			switch (state) {
				case FORWARD:
				case STOP:
				case BACKWARD:
					// do nothing if already at max backward speed
					if (right == left && left <= -50){
						right = left = -50;
						break;
					}
					right -= 25;
					left -= 25;
					if (right < 0)
						state = BACKWARD;
					else if (right == 0)
						state = STOP;
					else
						state = FORWARD;
					break;
				case LEFT:
				case RIGHT:
					right += (left - right)/2;
					if (right == 80)
						right = 75;
					left  = right;
					left -= 25;
					right -= 25;
					state = BACKWARD;
			}
			if (right == left && left <= -50){
				right = left = -50;
			}
			break;
		case LEFT:
			if ((right - left) == 40)
				break;
			switch (state) {
				case RIGHT:
					right += (left - right)/2;
					left  = right;
				case FORWARD:
					if (right == 100 && left == 100) {
						right = 90;
						left  = 90;
					}
				case LEFT:
				case BACKWARD:
				case STOP:
					right += 10;
					left  -= 10;
					if (right == 110){
						right -= 10;
						left -= 10;
					}
					state = LEFT;
					break;
			}
			break;
		case RIGHT:
			if ((left - right) == 40)
				break;
			switch (state) {
				case LEFT:
					right += (left - right)/2;
					left  = right;
				case RIGHT:
				case FORWARD:
					if (right == 100 && left == 100) {
						right = 90;
						left  = 90;
					}
				case BACKWARD:
				case STOP:
					right -= 10;
					left  += 10;
					if (left == 110){
						right -= 10;
						left -= 10;
					}
					state = RIGHT;
					break;
			}
			break;
		case STOP:
			right = 0;
			left = 0;
			state = STOP;
			break;
		}
//radhika start
		if(m_pMainApp->m_useSharedControl != FALSE){
			SetUserRequest(cond);
		}
//end radhika

	switch (state) {
		case LEFT:
		case RIGHT:	
	//		SetTimer(NULL, 1, 1000, (TIMERPROC) TimerProc_Wrapper);
		default:
			CString msg;
			TRACE2("DRIVE {Left %d} {Right %d}\n", left, right);
			msg.Format("DRIVE {Left %d} {Right %d} {Normalized true}",left, right);
			m_pMainApp->SendUTMessage(msg);
	}
	TRACE1("OUT: %d", state);
}

//! Wrapper class for TimerProc. Circumfences requirement for static TimerProc function.
/*! The Timer implementation is based on CodeProject tutorial
http://www.codeproject.com/KB/cpp/SetTimer__non-static.aspx */
void CALLBACK StateControl::TimerProc_Wrapper( HWND hwnd, UINT uMsg, UINT_PTR idEvent, DWORD dwTime ) 
{
	//TRACE0("TimerProc_Wrapper\r\n");
	StateControl *pSomeClass = (StateControl*)pObject; // cast the void pointer

	pSomeClass->ProcessTimeout(hwnd, uMsg, idEvent, dwTime); // call non-static function

}
void CALLBACK StateControl::ProcessTimeout(HWND hwnd, UINT uMsg, UINT_PTR idEvent,DWORD dwTime) 
{	

	if (state != RIGHT && state != LEFT)
		return;
	right += (left - right)/2;
	left   = right;
	if (right < 0)
		state = BACKWARD;
	else if (right = 0)
		state = STOP;
	else
		state = FORWARD;

	CString msg;
	msg.Format("DRIVE {Left %d} {Right %d} {Normalized true}",left, right);
	m_pMainApp->SendUTMessage(msg);
	KillTimer(hwnd,idEvent);
}
void StateControl::Reset(void) {
	left = 0;
	right = 0;
	state = STOP;
}

StateControl::~StateControl(void)
{
	KillTimer(NULL,timer);
}

/****************************************************************************/
/************************** CODE ADDED BY RADHIKA ***************************/


void StateControl::SharedTransFun(int cond, int velocityLevel)
{
	TRACE3("SharedTransFun start\nState: %d\nRight: %d\nLeft: %d\n", state, right, left);

	//TRACE2("IN: %d %d",cond, state);
// First, calculate the new state:

	if (velocityLevel > 9){
		velocityLevel = 9;
	}
	if (velocityLevel < 0){
		velocityLevel = 0;
	}

	/*bool proceed;
	proceed = CheckCollision(cond);
	TRACE1("Collision checked. Proceed = %d\n", proceed);
	if(proceed == false){
		return;
	}*/

	switch (cond) {
		case FORWARD:
			switch (state) {
				case FORWARD:
				case STOP:
				case BACKWARD:
					// do nothing if already at max speed
					if (right == 50 && left == 50)
						break;
					// otherwise increase forward speed
					right += 25;
					left += 25;
					// update the state
					if (right < 0)
						state = BACKWARD;
					else if (right == 0)
						state = STOP;
					else
						state = FORWARD;
					break;
				case LEFT:
				case RIGHT:
					right += (left - right)/2;
					/*if (right == 90)
						right = 100;
					if (right == 80)
						right = 75;*/
					left  = right;
					if (right <= 0 || left <= 0)
						left = right = 25;
					state = FORWARD;
					//! \todo change this to go through STOP.
			}
			break;
		case BACKWARD:
			switch (state) {
				case FORWARD:
				case STOP:
				case BACKWARD:
					// do nothing if already at max backward speed
					if (right == left && left <= -50){
						right = left = -50;
						break;
					}
					right -= 25;
					left -= 25;
					if (right < 0)
						state = BACKWARD;
					else if (right == 0)
						state = STOP;
					else
						state = FORWARD;
					break;
				case LEFT:
				case RIGHT:
					right += (left - right)/2;
					if (right == 80)
						right = 75;
					left  = right;
					left -= 25;
					right -= 25;
					state = BACKWARD;
			}
			if (right == left && left <= -50){
				right = left = -50;
			}
			break;
		case LEFT:
			if (((right - left) == 40) /*&& (velocityLevel==9)*/)
				break;
			switch (state) {
				case RIGHT:
					right += (left - right)/2;
					left  = right;
				case FORWARD:
					if (right == 100 && left == 100) {
						right = 90;
						left  = 90;
					}
				case LEFT:
				case BACKWARD:
				case STOP:
					right += velocityLevel + 1;
					left  -= velocityLevel + 1;					
					if (right == 110){
						right -= 10;
						left -= 10;
					}
					state = LEFT;
					break;
			}
			break;
		case RIGHT:
			if (((left - right) == 40) /*&& (velocityLevel == 9)*/)
				break;
			switch (state) {
				case LEFT:
					right += (left - right)/2;
					left  = right;
				case RIGHT:
				case FORWARD:
					if (right == 100 && left == 100) {
						right = 90;
						left  = 90;
					}
				case BACKWARD:
				case STOP:
					right -= velocityLevel + 1;
					left  += velocityLevel + 1;				
					if (left == 110){
						right -= 10;
						left -= 10;
					}
					state = RIGHT;
					break;
			}
			break;
		case STOP:
			right = 0;
			left = 0;
			state = STOP;
			break;
		
	}

	TRACE3("SharedTransFun END\nState: %d\nRight: %d\nLeft: %d", state, right, left);


	switch (state) {
		case LEFT:
		case RIGHT:	
	//		SetTimer(NULL, 1, 1000, (TIMERPROC) TimerProc_Wrapper);
		default:
			CString msg;
			//TRACE2("DRIVE {Left %d} {Right %d}\n", left, right);
			msg.Format("DRIVE {Left %d} {Right %d} {Normalized true}",left, right);
			m_pMainApp->SendUTMessage(msg);
	}
	//TRACE1("OUT: %d", state);
}

bool StateControl::ModifyControl(int cond)
{
	if(cond == FORWARD){
		SharedTransFun(FORWARD, 9);
		SetUserRequest(cond);
		return false;
	}

	return true;
}

void StateControl::SetCollisionRisk(int colRisk[])
{
	ResetCollisionRisk();
	
	for(int i = 0; i < 5; i++){
		collisionRisk[i] = colRisk[i];
	}
	
}

void StateControl::ResetCollisionRisk()
{
	for(int i = 0; i < 5; i++){
		collisionRisk[i] = 0;
	}
	
}
bool StateControl::CheckCollision(int cond)
{
	for (int i=0; i<5; i++){
		TRACE2("collisionRisk[%d] = %d\n", i, collisionRisk[i]);
	}

	switch(cond){
		case FORWARD:
			if((collisionRisk[1] ==1)/* || (collisionRisk[2] == 1) || (collisionRisk[3] == 1)*/){
				TRACE0("Risk of collision: Front!\n");
				return false;
			}
				break;
		case RIGHT:
			if((collisionRisk[0] ==1) || (collisionRisk[1] ==1)){
				TRACE0("Risk of collision: Right!\n");
				/*if(state == RIGHT){
					cond = STOP;
					return true;
				}else*/{
					return false;
				}
			}
				break;
		case LEFT:
			if((collisionRisk[3] ==1) || (collisionRisk[4] ==1)){
				TRACE0("Risk of collision: Left!\n");
				/*if(state == LEFT){
					cond = STOP;
					return true;
				}else*/{
					return false;
				}
			}
				break;
	}
	return true;
}

int StateControl::GetState(void)
{
	return state;
}

void StateControl::SetUserRequest(int cond)
{
	userRequest = cond;
}
int StateControl::GetUserRequest(void)
{
	return userRequest;
}
//end radhika