/*******************************************************************************

	AlphaBot

		Behavior.c
*******************************************************************************/

#include <stdlib.h>
#include "AlphaBot.h"
//------------------------------------------------------------------------------

void	Behavior(void)	//-- AI Behavior - read sensors, make desicion, set tasks
{
	// *** Check sensors, perform reflectory behavior if required ***

	if( !MODE_DEBUG )		//-- TESTING
	{

		if( IS_TILTED )	//-- HW/capacitor provide Shmidt trigger/delay - no need to wait/confirm
			Behave_Tilted();

		if( IS_DARK )
			Behave_Dark();

		if( SENSOR_IR_SHORT_PUSHED )
		{
			Show(S_BUMP_IR_SHORT);
//			SetTask_Reverse(ANYSIDE);
			Reverse(ANYSIDE);
		}
		IF_BUTTON_PUSHED( BUMPER_LEFT )
		{
			Show(S_BUMP_LEFT);
//			SetTask_Reverse(LEFT);
			Reverse(LEFT);
		}
		IF_BUTTON_PUSHED( BUMPER_LEFT )
		{
			Show(S_BUMP_RIGHT);
//			SetTask_Reverse(RIGHT);
			Reverse(RIGHT);
		}
		IF_BUTTON_PUSHED( BUMPER_REAR )
		{
			// clear SetTask_Reverse - pending

			Side	eSide = (Side)(rand()%2);

			Show(S_BUMP_REAR);

			if( eSide == LEFT )
				Spin(-3,3,4);
			else
				Spin(3,-3,4);
		}
	}

	// ... low-bat - PENDING


	// *** Normal life ***

	if( MODE_DEBUG )		//-- TESTING
	{
		IF_BUTTON_PUSHED( BUMPER_LEFT )
			cSpeed[LEFT]++;
//			MOTOR_L_SPEED_F += 5;
			

		IF_BUTTON_PUSHED( BUMPER_RIGHT )
			cSpeed[LEFT]--;
//			MOTOR_L_SPEED_B += 5;

//		IF_BUTTON_PUSHED( BUMPER_REAR )
	}
	else
	if( MODE_ARTISTIC )		//-- ARTISTIC
		Dance();
	else				//-- NOTMAL STATE
		Behave_Wander();

} // Behavior
//------------------------------------------------------------------------------

void	Behave_Tilted(void)
{
	AllStop(HARD);

	while( IS_TILTED )
	{	
		Show(S_TILTED);
		Delay(3000);
	}
	Diagnostic(TRUE);

} // Behave_Tilted
//------------------------------------------------------------------------------

void	Behave_Dark(void)
{
	_idat	ulong	ulDoUntil;

	//-- Confirm darkness
	Delay(1000);
	if( ! IS_DARK )
		return;

	//--- Dark operating mode

	AllStop(SOFT);

	Show( S_TIME2SLEEP );	//-- Signal acknowledgement of darkness

		//-- Drive off the clear area, until hit something

	cSpeed[LEFT] = cSpeed[RIGHT] = SPEED_NORMAL;

	ulDoUntil = ulTickCount + 20 SECONDS;
 	while( ulTickCount < ulDoUntil )
	{
		MaintainSpeed();

		if( SENSOR_IR_SHORT_PUSHED )
			break;
		IF_BUTTON_PUSHED( BUMPER_LEFT )
			break;
		IF_BUTTON_PUSHED( BUMPER_LEFT )
			break;
	}
	AllStop(SOFT);

	do
	{
		while( IS_DARK )
		{
			//-- Snore once in a while
	//		f( (ulTickCount % 5 MINUTES) == 0 )	//-- every 5 nimutes or so
			if( (ulTickCount % (10 SECONDS)) == 0 )	//-- every 10 sec or so (testing.)
				Show( S_SNORE );
		}
		//-- Confirm exit from darkness
		Delay(1000);
	}
	while( IS_DARK );

	//-- End of darkness
	Show( S_WAKEUP );
	Delay(1000);
	Diagnostic(TRUE);

} // Behave_Dark
//-----------------------------------------------------------------------------------------

void	Behave_Wander (void)
{
	//	gets executed when there are no tasks in the queue
	//	for now - 
	//	drive in dandom directions for random periods of time,
	//	spin, scan for obstacles, sound and lights once in a while.

	static	_idat	unsigned long	ulUntilTick = 0;

	if( ulTickCount >= ulUntilTick )
	{
/*		if( !(ulTickCount % 8) )	//-- 1/8 of the time
			Spin( rand()%20, rand()%20, rand()%20 );
		else
		{
			//-- Set each wheel speed to maintain
*/			cSpeed[LEFT] = 6 + (rand() % 6);

			if( (ulTickCount%3) == 0 )	//-- Drive strait for 1/3 of the time
				cSpeed[RIGHT] = cSpeed[LEFT];
			else
				cSpeed[RIGHT] = 6 + (rand() % 6);
//		}

		ucLrSensorDistance = ScanAt( ( rand() % (HEAD_RESETOFFSET*2-4) ) - HEAD_RESETOFFSET-2 );

//		if( !(ulTickCount % 5 ) )	//-- 1/5
			Show( S_RANDOM );

		ulUntilTick = ulTickCount + ( 3 SECONDS + rand()%(5SECONDS) );
	}
} // Behave_Wander
//-----------------------------------------------------------------------------------------