/***********************************************************************/
/** @name drivedemo.c 
    Contains application specific drive routines/ drive process.    
    
    @author Birgit Graf, UWA, 1998, modified 2000 (Mk3/4)
*/
/***********************************************************************/

/*@{*/



#include "imageglobal.h"
#include "servos.h"
#include "drivedemo.h"
#include "driveglobal.c"


#define DRIVE_AREA_LENGTH 2		/* size of drive area (quadratic) in m */
int use_area = FALSE;

#define LOOK_AT_BALL_CYCLES 3		/* time to look at ball outside driving area */


/***********************************************************************/
/** Say whether player is goalkeeper.
 */
/***********************************************************************/

int I_am_goalie()
{
  return FALSE;
}


/***********************************************************************/
/** Set field player movement.
    Move robot and output text (drive status) on LCD.
    Activate robot movement according to flags end_flag,
    obstacle_flag, got_ball_flag and see_ball_flag, concerning
    different priorities.

    Process started by main().
    @see main()
*/
/***********************************************************************/

void PPdrive_field()
{
  PositionType pos, ball;
  
  while(!end_flag)
  {
    /*     LCDPrintf("control\n"); */
    
    LCDSetPos(0, 10);
    LCDPrintf("o%ds%dg%d\n", obstacle_flag, see_ball_flag, got_ball_flag);
    

    /* check if ball is in front of robot -> kick */
    if (see_ball_flag || got_ball_flag)
    {
			VWGetPosition(vw, &pos);
			get_ball_coord(&ball); /* ball position */

			if (hypot(ball.x - pos.x, ball.y - pos.y) < GOT_BALL)
			{
				kick_ball();			
				got_ball_flag = FALSE;
			}
    }
    
    if (obstacle_flag)		/* ran into obstacle -> stop */
    {
			if (!sim_motors)
			{
				obstacle_flag = FALSE;
				drive_stop();
			}
    }
		else		/** no obstacle in front */
		{
			if (see_ball_flag)	/* see ball -> drive to it */
			{
			  /*				see_ball_flag = FALSE;*/ /* driving to current ball position
															 -> don't consider it again */
				if (!sim_motors)
				{
					/* ball inside region? */
					if (((fabs(ball.x) < DRIVE_AREA_LENGTH && fabs(ball.y) < DRIVE_AREA_LENGTH)) ||
						!use_area)
						start_curve_to_ball(pos, ball, FALSE); /* drive to ball */
					else
    				if (!sim_motors)
							drive_stop();
				} /* if (sim_motors) */
			}	
			else /* (if see ball) no ball in sight */
			{
    		if (!sim_motors)
					drive_stop();
			}
		}	/* else (if obstacle) */
		OSReschedule();	
  }	/* while */
  end_flag += 1;
  OSKill(0);
}
  
  
/*@}*/