/***********************************************************************/
/** GOALIE: Set 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_goal()
{
  PositionType ball;
  int factor = 0;

  while(!end_flag)
  {
    LCDSetPos(0, 10);
    LCDPrintf("o%ds%d\n", obstacle_flag, see_ball_flag);

    if (obstacle_flag)
    {
      LCDSetPos(1, 10);
      LCDPrintf("Wall!!\n");
      LCDSetPos(2, 10);
      LCDPrintf("     \n");
      LCDSetPos(3, 10);
      LCDPrintf("     \n");
      
      VWSetSpeed(vw, 0.0, 0.0);
    }
    else 
      if (see_ball_flag)
      {
	AUBeep();

	get_ball_coord(&ball);
	
	LCDSetPos(1, 10);
        LCDPrintf("Ball:\n");
        LCDSetPos(2, 10);
        LCDPrintf("(%d,\n", (int)(100 * ball.x));
        LCDSetPos(3, 10);
        LCDPrintf("% 2d) \n", (int)(100 * ball.y));
        
        factor = 3;             /* allow factor pictures without ball
                                   after driving to it */
        start_goal_line(ball);
      }
      else                      /* no ball in sight */
      {
        LCDSetPos(1, 10);
        LCDPrintf("     \n");
        LCDSetPos(2, 10);
        LCDPrintf("     \n");
        LCDSetPos(3, 10);
        LCDPrintf("     \n");

        if (factor)
          factor --;
        else
        {
	  ball.x = 0.1;
	  ball.y = 0.0;
	  start_goal_line(ball);   /* lost ball -> drive back to middle of goal */
          if (VWDriveDone(vw))
          {
            LCDSetPos(2, 10);
            LCDPrintf("MID  \n");
          }
        }
      }
    OSReschedule();
  }
  end_flag += 1;
  OSKill(0);
}

/*@}*/