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

	AlphaBot

		Diagnostic.c

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

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

bool	Diagnostic( bool bFull )	//-- Self-Diagnostic routine, Full and Quick
{
	_idat	bool	bRet = TRUE;

		//-- *** Quick self-diagnostick ***
	//-- Battery level check
	// ...

		//-- *** Full self-diagnostic ***

	if( bFull )
	{
	    do
	    {
		_idat	uchar	i;

		bRet = TRUE;
		Show(S_DIAG_BEGIN);

		//-- Sensory system check - 3X Bumpers
		if( BUMPER_LEFT  == BTN_ON ||
		    BUMPER_RIGHT == BTN_ON ||
		    BUMPER_REAR  == BTN_ON
		  )
		{
			CommStatusCode( STAT_ERROR, ST_DIAG_BUMBER_TEST );
			bRet = FALSE;
		}

		//-- Head posiion - to Home
		HeadReset();

		//-- Left drive - spin back and forth

		//-- Rihgt drive - spin back and forth

		//-- Ensure SrIR sensor is initialized by this time (this one may take some time)
		for( i=0; i<5*5 && SENSOR_IR_SHORT == 1; i++ )	//-- 5 sec
			Delay(200);
		if( SENSOR_IR_SHORT == 1 )	// sensor is stil not initialized( or obtruction is in front)
		{
			CommStatusCode( STAT_ERROR, ST_DIAG_IR_SHORT_TEST );
			bRet = FALSE;
		}
		if( bRet == FALSE )
		{
			Delay(1000);
			Show(S_DIAG_ERROR);
			Delay(1000);
			CommStatusCode( STAT_ERROR, ST_DIAG_WAITFORFIX );
			Delay(5000);
			Show(S_WAITING);  // wait for problem to be fixed
			Delay(2000);
		}

	    } while( bRet == FALSE );

		if( bRet == TRUE )
			Show(S_DIAG_OK);
		else
			Show(S_DIAG_ERROR);
	}
	return bRet;

} // Diagnostic
//-----------------------------------------------------------------------------------------

rom const unsigned char MorseDigit [10] = { 0, 1, 3, 7, 15, 31, 30, 28, 24, 16 };
/*
0   ----- 
1   .---- 
2   ..--- 
3   ...-- 
4   ....- 
5   ..... 
6   -.... 
7   --... 
8   ---.. 
9   ----. 
*/
#define	MORSE_ERR_FREQ		750
#define	MORSE_ERR_DUR_DOT	80
#define	MORSE_ERR_DUR_DASH	300
#define	MORSE_ERR_DUR_SPACE	80
//------------------------------------------------------------------------------

void	Morse(bool d) { LED_TWR_RED = LED_ON; Beep( MORSE_ERR_FREQ, d?MORSE_ERR_DUR_DOT:MORSE_ERR_DUR_DASH ); LED_TWR_RED = LED_OFF; Delay(MORSE_ERR_DUR_SPACE); }
//------------------------------------------------------------------------------

void	CommStatus_Wrapper( StatusCodeType eType )	//-- internal use
{
	switch( eType )
	{
		case STAT_INFO:
			//-- 3 shorts
			Morse(1); Morse(1); Morse(1);
			break;
		case STAT_WARN:
			//-- 3 longs
			Morse(0); Morse(0); Morse(0);
			break;
		default:	// STAT_ERROR
			//-- SOS
			Morse(1); Morse(1); Morse(1);
			Morse(0); Morse(0); Morse(0);
			Morse(1); Morse(1); Morse(1);
	}
} // CommStatus_Wrapper
//------------------------------------------------------------------------------

void	CommStatus_Digit( uchar ucDigit )	//-- internal use
{
	_idat	uchar	i, ucMorseDigit = MorseDigit[ucDigit%10];

	for( i=0; i<5; i++ )
	{
		Morse( ucMorseDigit & 1 );
		ucMorseDigit >>= 1;
	}

} // CommStatus_Digit
//------------------------------------------------------------------------------
/*
void	CommStatusCode( StatusCodeType eType, uchar ucCode )	//-- Communicate Error Code using Morse Code
{
	_idat	uchar	i;

	Delay(1000);
	CommStatus_Wrapper(eType);
	Delay(1000);

	//-- Communicate error code digit, 3 times
	for( i=0; i<3; i++ )
	{
		CommStatus_Digit(ucCode);
		Delay(1000);
	}
	CommStatus_Wrapper(eType);
	Delay(1000);

} // CommStatusCode
//-----------------------------------------------------------------------------------------
*/
void	CommStatusCode( StatusCodeType eType, ulong ulCode )
{
	_idat	ulong	ulDivider;
	_idat	uchar	i, ucDigit;
	_idat	bool	bFirst = TRUE;

	Delay(1000);
	CommStatus_Wrapper(eType);
	Delay(1000);

	for( ulDivider = 1000000000L; ulDivider >= 1; ulDivider /= 10 )
	{
		ucDigit = ulCode / ulDivider;
		if( ucDigit > 0 || !bFirst || ulDivider == 1 )
		{
			for( i=0; i<3; i++ )
			{
				CommStatus_Digit(ucDigit);
				Delay(1000);
			}
			bFirst = FALSE;
		}
		ulCode %= ulDivider;
	}

	CommStatus_Wrapper(eType);
	Delay(1000);

} // CommStatusCodeEx
//-----------------------------------------------------------------------------------------