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

	AlphaBot

		Drive.c

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

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

void	Drive(void)
{
	if( Stalled() )
		return;
	else
		MaintainSpeed();
} // Drive
//-----------------------------------------------------------------------------------------

bool	MaintainSpeed(void)
{
	static	_idat	ulong	ulNextSpeedAdjsut = SPEED_ADJUSTPERIOD;

	if( ulTickCount >= ulNextSpeedAdjsut	//-- time has come
//	    &&
//	    D_JMP == BTN_ON			//-- moving is enabled
	  )
	{
		AdjustWheelSpeed(LEFT);
		AdjustWheelSpeed(RIGHT);

		ulNextSpeedAdjsut = ulTickCount + SPEED_ADJUSTPERIOD;

		return TRUE;	//-- speed adjustment attempt has been made
	}
	else
		return FALSE;	//-- no adjsutment at this time

} // MaintainSpeed
//-----------------------------------------------------------------------------------------

_inline void	Drive_ChangeLoad( Side eSide, char cValue )
{
	Drive_SetLoad(eSide, Drive_GetLoad(eSide) + cValue );
} // Drive_ChangeLoad
//-----------------------------------------------------------------------------------------

void	AdjustWheelSpeed( Side eSide )	//-- internal use function
{
	if( cSpeed[eSide] == 0  &&	//-- idle - no further calculations
	    ( ( eSide == LEFT  && MOTOR_L_SPEED_F == 0 && MOTOR_L_SPEED_B == 0 )
	      ||
	      ( eSide == RIGHT && MOTOR_R_SPEED_F == 0 && MOTOR_R_SPEED_B == 0 )
	    )
	  )
		return;
	else
	if( abs(cSpeed[eSide]) > 0 &&	//-- check if adjustment lags and we are going in wrong direction
	    sign(cSpeed[eSide]) != sign(Drive_GetLoad(eSide))	//-- going in wrong direction?
	  )							//-- slow-down in chunks of 10
		if( Drive_GetLoad(eSide) < 0  )
			Drive_ChangeLoad(eSide, 10);
		else
			Drive_ChangeLoad(eSide, -10);
	else
	{
		_idat	uchar	ucDesiredSpeed = min( max(abs(cSpeed[eSide]),SPEED_MIN), SPEED_MAX );

			//-- timer counts between wheel clicks, in CPU cycles
		_idat	long	lActualCycleCount  = 
					( ulSpeed_CurTick[eSide] == 0 || ulSpeed_PrevTick[eSide] == 0 || ulTickCount - ulSpeed_CurTick[eSide] > SPEED_ADJUSTPERIOD * 2 )
					? MAX_LONG
					: (long)(ulSpeed_CurTick[eSide] - ulSpeed_PrevTick[eSide]) * (long)(0xFFFF-TIMER_VALUE) + ((long)uiSpeed_CurCount[eSide]-TIMER_VALUE) - ((long)uiSpeed_PrevCount[eSide]-TIMER_VALUE);

		_idat	long	lDesiredCycleCount = 
					( ucDesiredSpeed == 0 )
					? MAX_LONG 
					: (long)(0xFFFF-TIMER_VALUE)* 1 SECONDS / ucDesiredSpeed; // * (ulong)(SPEED_MAX-ucDesiredSpeed); // : 3*SPEED_ADJUSTPERIOD*(0xFF-TIMER_VALUE);

			//-- Caclulate speed error, in CPU cycles: >0 =too fast, <0 =too slow
		_idat	long	lSpeedError = lDesiredCycleCount - lActualCycleCount;

		_idat	char	cLoadChange  =	Drive_CalcLoadChange( lSpeedError );

		if( cLoadChange > 0  )
		{
				//-- ensure proper sign of LoadChange
				//-- depending on desired and actual directions

			if( lActualCycleCount < lDesiredCycleCount )	//-- too fast? - slow down
				cLoadChange = -cLoadChange;

			if( Drive_GetLoad(eSide) < 0  )			//-- moving backward? - change sign
				cLoadChange = -cLoadChange;

				//-- Set calcualted load change

			Drive_ChangeLoad(eSide, cLoadChange);

		}
		else
			if( cSpeed[eSide] == 0 ) //-- Need (eventualy) to stop
				if( eSide == LEFT )
					MOTOR_L_SPEED_F = MOTOR_L_SPEED_B = 0;
				else
					MOTOR_R_SPEED_F = MOTOR_R_SPEED_B = 0;
	}
} // AdjustSpeed
//-----------------------------------------------------------------------------------------

void	AllStop(StopMethod eStopMenthod)
{
	_idat	ulong	ulDoUntil;

	HeadStop();

	cSpeed[RIGHT] = cSpeed[LEFT] = 0;

	switch( eStopMenthod )
	{
		case MEDIUM:
			MOTOR_L_SPEED_F /=2;
			MOTOR_R_SPEED_F /=2;
			MOTOR_L_SPEED_B /=2;
			MOTOR_R_SPEED_B /=2;
			Delay(500);
		case HARD:
			break;
		default: // SOFT :
			ulDoUntil = ulTickCount + 3 SECONDS;
			while( (MOTOR_L_SPEED_F != 0	||	//-- slow down, slowly
				MOTOR_R_SPEED_F != 0	||
				MOTOR_L_SPEED_B != 0	||
				MOTOR_R_SPEED_B != 0
			       ) && ulTickCount < ulDoUntil
			     )
				MaintainSpeed();
	}
	MOTOR_L_SPEED_F = MOTOR_R_SPEED_F = MOTOR_L_SPEED_B = MOTOR_R_SPEED_B = 0;
} // AllStop
//-----------------------------------------------------------------------------------------

void	Reverse( Side eSide )
{
	_idat	ulong	ulDriveUntilClicks[2];
//	_idat	Side	eOtherSide;
	_idat	ulong	ulDoUntil;
	_idat	char	cGoThere;
	
	SetTwrColor(TWR_RED);
	SetBeeperFreq(700);
	BEEPER_ON;

	AllStop(MEDIUM);

	BEEPER_OFF;
	SetTwrColor(TWR_OFF);

	if( IS_TILTED )
	{
		Behave_Tilted();
		return;
	}

	ulDriveUntilClicks[LEFT] = ulMileage[LEFT] + 4;
	ulDriveUntilClicks[RIGHT] = ulMileage[RIGHT] + 4;

	cSpeed[LEFT] = cSpeed[RIGHT] = -SPEED_NORMAL;	//-- drive bacward

	ulDoUntil = ulTickCount + 7 SECONDS;
 	while(	(ulMileage[LEFT] < ulDriveUntilClicks[LEFT] ||
		 ulMileage[RIGHT] < ulDriveUntilClicks[RIGHT]
		) && ulTickCount < ulDoUntil
	     )
		MaintainSpeed();

	AllStop(MEDIUM);

	if( IS_TILTED )
	{
		Behave_Tilted();
		return;
	}

	if( eSide == ANYSIDE )	//-- Choose side, if required
	{
		cGoThere = Sweep();
		eSide = (cGoThere<0) ? LEFT : RIGHT; 
	}
	else
	{
		cGoThere = (eSide == LEFT) ? -HEAD_RESETOFFSET/2 : HEAD_RESETOFFSET/2;
		ScanAt(cGoThere);
	} 
	Show(S_GOTHERE);

	if( eSide == LEFT )
		Spin( -SPEED_NORMAL, SPEED_NORMAL, abs(cGoThere)/4); //-- more presise - pending
	else
		Spin( SPEED_NORMAL, -SPEED_NORMAL, abs(cGoThere)/4); //-- more presise - pending

	AllStop(HARD);
/*
	eOtherSide = (eSide == LEFT) ? RIGHT : LEFT;

	ulDriveUntilClicks[eOtherSide] = ulMileage[eOtherSide] + abs(cGoThere)/4; //-- more presise - pending

	Spin(
	cSpeed[eSide] = -SPEED_NORMAL;	//-- spin
	cSpeed[eOtherSide] = SPEED_NORMAL;

	ulDoUntil = ulTickCount + 6 SECONDS;
	while(	ulMileage[eOtherSide] < ulDriveUntilClicks[eOtherSide] &&
		ulTickCount < ulDoUntil
	     )
	{
		if(  MaintainSpeed() && cHeadPosition != 0 )
			HeadMove(eOtherSide);	//-- gradually streighten the head
	}
*/
	HeadMove(0);
	AllStop(HARD);

	ScanAt(0);	//-- retrun head to strait position

} // Reverse
//-----------------------------------------------------------------------------------------

bool	Stalled(void)
{
	_idat	Side	eSide = ANYSIDE;
	_idat	char	cCurSpeed;

	if( IS_WHEEL_STALLED(LEFT) )
	{
		eSide = LEFT;
		cCurSpeed = cSpeed[LEFT];
	}
	else
	if( IS_WHEEL_STALLED(RIGHT) )
	{
		eSide = LEFT;
		cCurSpeed = cSpeed[LEFT];
	}
	if( eSide == ANYSIDE )
		return FALSE;	//-- not stalled

	AllStop(HARD);

	Show(S_STALLED);

	if( eSide == LEFT )
		Spin( -cCurSpeed, 0, 6+(rand()%18) );
	else
		Spin( 0, -cCurSpeed, 6+(rand()%18) );

	return TRUE;

} // Stalled
//-----------------------------------------------------------------------------------------

void	Spin( char cSpeedLeft, char cSpeedRight, uint uiClicks ) //-- uiClicks of max speed's side
{
	_idat	Side	eSide = (cSpeedLeft>cSpeedRight) ? LEFT : RIGHT;
	_idat	ulong	ulDriveUntilClicks,
			ulDoUntil;

	cSpeed[LEFT]  = cSpeedLeft;
	cSpeed[RIGHT] =	cSpeedRight;

	ulDriveUntilClicks = ulMileage[eSide] + uiClicks;

	ulDoUntil = ulTickCount + 10 SECONDS;
	while(	ulMileage[eSide] < ulDriveUntilClicks &&
		ulTickCount < ulDoUntil
	     )
		MaintainSpeed();
} // Spin
//-----------------------------------------------------------------------------------------

int	Drive_GetLoad( Side eSide )
{
	if( eSide == LEFT )
		if( MOTOR_L_SPEED_F > 0 )
			return MOTOR_L_SPEED_F;
		else
			return -MOTOR_L_SPEED_B;
	else	// RIGHT
		if( MOTOR_R_SPEED_F > 0 )
			return MOTOR_R_SPEED_F;
		else
			return -MOTOR_R_SPEED_B;
} // Drive_GetLoad
//-----------------------------------------------------------------------------------------

void	Drive_SetLoad( Side eSide, int iValue )
{
	if( iValue == 0 )
	{
		if( eSide == LEFT )
			MOTOR_L_SPEED_F = MOTOR_L_SPEED_B = 0;
		else
			MOTOR_R_SPEED_F = MOTOR_R_SPEED_B = 0;
		return;
	}
	if( iValue > MOTOR_MAX_LOAD )
		iValue = MOTOR_MAX_LOAD;
	else
	if( iValue < -MOTOR_MAX_LOAD )
		iValue = -MOTOR_MAX_LOAD;
	else
	if( iValue > 0 && iValue < MOTOR_MIN_LOAD )
		if( Drive_GetLoad(eSide) <= 0 )
			iValue = MOTOR_MIN_LOAD;	// accel
		else
			iValue = 0;			// descel
	else
	if( iValue < 0 && iValue > -MOTOR_MIN_LOAD )
		if( Drive_GetLoad(eSide) >= 0 )
			iValue = -MOTOR_MIN_LOAD;	// accel
		else
			iValue = 0;			// descel

	if( iValue == 0 )
	{
		if( eSide == LEFT )
			MOTOR_L_SPEED_F = MOTOR_L_SPEED_B = 0;
		else
			MOTOR_R_SPEED_F = MOTOR_R_SPEED_B = 0;
		return;
	}

	if( eSide == LEFT )
		if( iValue > 0 )
		{
			MOTOR_L_SPEED_B = 0;
			MOTOR_L_SPEED_F = iValue;
		}
		else
		{
			MOTOR_L_SPEED_F = 0;
			MOTOR_L_SPEED_B = -iValue;
		}
	else		// RIGHT
		if( iValue > 0 )
		{
			MOTOR_R_SPEED_B = 0;
			MOTOR_R_SPEED_F = iValue;
		}
		else
		{
			MOTOR_R_SPEED_F = 0;
			MOTOR_R_SPEED_B = -iValue;
		}
} // Drive_SetLoad
//-----------------------------------------------------------------------------------------

uchar	Drive_CalcLoadChange( long lSpeedError )
{
//	uchar	ucCurLoadValue;

	lSpeedError = labs(lSpeedError);

	if( lSpeedError <= SPEED_ERROR_ALLOW )
		return 0;
	else
		return min( /*5*/ 10, lSpeedError / SPEED_ERROR_ALLOW );

/*	if( lSpeedError <= SPEED_ERROR_ALLOW*2 )
		return 2;
	else

	ucCurLoadValue = abs(Drive_GetLoad(eSide));

	if( ucCurLoadValue <  60 || lSpeedError <= SPEED_ERROR_ALLOW*3 )
		return	2;	//-- slow speeds - small increments
	else
	if( ucCurLoadValue <  80 || lSpeedError <= SPEED_ERROR_ALLOW*5)
		return	3;	//-- medium speeds - medium increments
	else
	if( ucCurLoadValue < 100 || lSpeedError <= SPEED_ERROR_ALLOW*5 )
		return	5;	//-- faster speeds - larger increments
	else
	if( ucCurLoadValue < 150 )
		return	10;	//-- fast speeds - large increments
	else
		return	20;	//-- ...
*/
/*
	if( abs(cSpeed[eSide]) <=6 )
		return 3;
	else
	if( abs(cSpeed[eSide]) <=10 )
		return 7;
	else
		return 15;
*/
} // Drive_CalcLoadChange
//-----------------------------------------------------------------------------------------

void _interrupt(0) _using(2) WheelClick_Left(void) //-- Measure Left wheel speed
{
	ulSpeed_PrevTick[LEFT] = ulSpeed_CurTick[LEFT];
	uiSpeed_PrevCount[LEFT] = uiSpeed_CurCount[LEFT];

	ulSpeed_CurTick[LEFT] = ulTickCount;
	uiSpeed_CurCount[LEFT] = TIMER_GET;

	ulMileage[LEFT]++;

	uiSpeed_StalledSheelCounter[LEFT] = FALSE;

} // interrupt WheelClick_Left
//-----------------------------------------------------------------------------------------

void _interrupt(2) _using(3) WheelClick_Right(void) //-- Measure rihght wheel speed
{
	ulSpeed_PrevTick[RIGHT] = ulSpeed_CurTick[RIGHT];
	uiSpeed_PrevCount[RIGHT] = uiSpeed_CurCount[RIGHT];

	ulSpeed_CurTick[RIGHT] = ulTickCount;
	uiSpeed_CurCount[RIGHT] = TIMER_GET;

	ulMileage[RIGHT]++;

	uiSpeed_StalledSheelCounter[RIGHT] = FALSE;

} // interrupt WheelClick_Right
//-----------------------------------------------------------------------------------------