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

	AlphaBot

		Scan.c

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

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

uchar	ScanAt ( char cPosition )
	//--	In:	cPosition -=LeftClicks, +=RightClicks, 0=Front/Center,
	//--	Out:	LrSensor's [0-3] or -1 if error
{
	if( D_JMP == BTN_OFF )			//-- moving is disabled?
		return 0;

	if( cPosition != cHeadPosition )
	{
		_idat	Side	eSide = ( cHeadPosition < cPosition ) ? RIGHT : LEFT;
		while( cHeadPosition != cPosition )
			HeadMove( eSide );
	}
	return LrSensor();

} // ScanAt
//-----------------------------------------------------------------------------------------

char	Sweep(void)	//-- does 360 degrees sweep, 12 measurements, to find an open path to go
{
	_idat	char	cPos,
			cMaxPos = ((rand()%2)==0) ? -HEAD_RESETOFFSET/2 : HEAD_RESETOFFSET/2; // random side (in case of error)
	_idat	uchar	ucDistnace,
			ucMaxDistnace = 0;

	if( ! Head2Home() )
		return cMaxPos;	//-- pick any

	for( cPos = HEAD_RESETOFFSET; cPos > -HEAD_RESETOFFSET; cPos -= 4 )
	{
		ucDistnace = ScanAt(cPos);
/*
switch(ucDistnace)
{ case 0:	SetTwrColor( TWR_ON );	break;
  case 1:	SetTwrColor( TWR_RED );	break;
  case 2:	SetTwrColor( TWR_BLUE ); 	break;
  case 3:	SetTwrColor( TWR_YELLOW );	break;
  default:	SetTwrColor( TWR_OFF );	break;
} 
Delay(1500);
SetTwrColor( TWR_OFF );
*/
		if( ucDistnace > ucMaxDistnace ) // in future use maxdistance and widest view
		{
			ucMaxDistnace = ucDistnace;
			cMaxPos = cPos;
		}
	}
	ScanAt(cMaxPos);	//-- indicate found position

	return cMaxPos;
} // Sweep
//-----------------------------------------------------------------------------------------

uchar	LrSensor(void)
	//-- Long Range IR Sensor measurement
	//--	returns proximity to the obstacle:
	//--		1 = obstacle is very close	    	1'
	//--		2 = obstacle detected at medium range	2-6'
	//--		3 = obstacle detected at long range	6-10'
	//--		0xFF = no obstacles in range 	     	>10'
	//--		0 = Error (not used)
{
	Show(S_IR_LONG_SCAN);	//-- let sensor to adjust, and for LED to be visible

	if( SENSOR_IR_LONG_3 )
		return 1;
	else
	if( SENSOR_IR_LONG_2 )
		return 2;
	else
	if( SENSOR_IR_LONG_1 )
		return 3;
	else
		return 0xFF;

} // LrSensor
//-----------------------------------------------------------------------------------------

rom const uchar	ucHeadLatchR [] = { 0x05, 0x06, 0x0A, 0x09 };
rom const uchar	ucHeadLatchL [] = { 0x09, 0x0A, 0x06, 0x05 };

void	HeadMove( Side eSide )	//-- Moves head 1 click (4-phase) (internal use function)
{
	_idat	uchar	i;

	for( i=0; i < 4; i++ )
	{
		Delay(HEAD_MOVEDELAY);

		if( eSide == RIGHT )
		{
			HEAD_MOTOR1 = ucHeadLatchR[i] & 0x01;
			HEAD_MOTOR2 = ucHeadLatchR[i] & 0x02;
			HEAD_MOTOR3 = ucHeadLatchR[i] & 0x04;
			HEAD_MOTOR4 = ucHeadLatchR[i] & 0x08;
		}
		else	//== LEFT
		{
			HEAD_MOTOR1 = ucHeadLatchL[i] & 0x01;
			HEAD_MOTOR2 = ucHeadLatchL[i] & 0x02;
			HEAD_MOTOR3 = ucHeadLatchL[i] & 0x04;
			HEAD_MOTOR4 = ucHeadLatchL[i] & 0x08;
		}
	}
	if( eSide == RIGHT )
		cHeadPosition++;
	else	//== LEFT
		cHeadPosition--;
} // HeadMove
//-----------------------------------------------------------------------------------------

bool	Head2Home(void)
{
	_idat	uchar	i = 0;

	//-- Give it a small left jump first
	HeadMove(LEFT);

	//-- Turn right until HeadHome sensor is hit
	while(	SENSOR_HEADHOME == BTN_OFF &&
		++i < HEAD_MAXRESETMOVES
	      )
		HeadMove(RIGHT);

	cHeadPosition = HEAD_RESETOFFSET;

	if( i >= HEAD_MAXRESETMOVES )	// HH sensor was never hit
	{
		CommStatusCode( STAT_ERROR, ST_HEADHOMEERROR );
		return FALSE;
	}
	else
		return TRUE;
} // Head2Home
//-----------------------------------------------------------------------------------------

bool	HeadReset(void)
{
	_idat	uchar	i = 0;

	if( D_JMP == BTN_OFF ) 			//-- moving is disabled?
		return TRUE;

	if( ! Head2Home() )
		return FALSE;

	//-- Center the head
	for( i=0; i < HEAD_RESETOFFSET; i++ )
		HeadMove(LEFT);

	return TRUE;

} // HeadReset
//-----------------------------------------------------------------------------------------

void	HeadStop(void)	//-- releases motor
{
	HEAD_MOTOR1 = HEAD_MOTOR2 = HEAD_MOTOR3 = HEAD_MOTOR4 = 0;

} // HeadStop
//-----------------------------------------------------------------------------------------