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

/*@{*/



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



/** flag to indicate location of ball (1 -> turn left, -1 -> turn right) */
int direction = 1;

/** radius of circle for goalie (> GOALWIDTH/2) */
#define radius  0.55

/** distance goal middle to circle middle */
#define goaldist 0.5

/** goalie: maximum angle to ball where robot is still allowed to
    drive ( atan (goaldist / GOALWIDTH/2 */
#define anglemax 1.12


/** home position of different players */ 
PositionType home[4][3] =
{{{LENGTH4, 0.2, 0.0},        {LENGTH2 - 0.3, WIDTH2 - 0.3, 0.0},  {0.3, WIDTH2 - 0.3, 0.0}},             /* lb */
 {{LENGTH4, -0.2, 0.0},       {LENGTH2 - 0.3, -WIDTH2 + 0.3, 0.0}, {0.3, -WIDTH2 + 0.3, 0.0}},            /* rb */
 {{3.0 * LENGTH4, 0.2, 0.0},  {LENGTH - 0.3, WIDTH2 - 0.3, 0.0},   {LENGTH2 + 0.3, WIDTH2 - 0.3, 0.0}},   /* lf */
 {{3.0 * LENGTH4, -0.2, 0.0}, {LENGTH - 0.3, -WIDTH2 + 0.3, 0.0},  {LENGTH2 + 0.3, -WIDTH2 + 0.3, 0.0}}}; /* rf */


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

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

int I_am_goalie()
{
  return (player_pos == POS_G);
}



/***********************************************************************/
/** Avoid obstacles.
    Function makes robot drive back in case there is an obstacle in front
    of it (wheels stalled or front-PSD-distance below obstacle_thresh).

    Called by PPdrive_field().
    @see PPdrive_field()
*/
/***********************************************************************/

void avoid_obstacle()
{
  PositionType pos;

  VWGetPosition(vw, &pos);

  /* ----- drive away from obstacle ----- */
  if (got_ball_flag) 
  {
    direction = (int) fsign(pos.y);
    
    /* got ball -> turn to kick it towards opponent's goal */
      VWDriveTurn(vw, (float) -direction * PI/4, 5.0 * ROT_SPEED); 

    while(!VWDriveDone(vw) && !(KEYRead() == KEY3)){}
  }
  
  /* drive back from wall */
  VWDriveStraight(vw, -0.05, LIN_SPEED);	
  while(!VWDriveDone(vw) && !(KEYRead() == KEY3) && !VWStalled(vw)){}
  
  got_ball_flag = FALSE;
  see_ball_flag = FALSE;		/* lost ball while driving
																back -> search again */
}



/***********************************************************************/
/** Search goal and drive to it.
    If ball is caught in front of robot, drive curve with fixed angle
    (don't want to loose ball) until facing goal, then kick it
    there. Kick ball as well as soon as goal is seen.

    Called by PPdrive_field().
    @see PPdrive_field()
*/
/***********************************************************************/

void drive_to_goal()
{
  PositionType pos;
  int x_middle = 0, y_middle = 0, goal_size = 0; /* parameters of goal */

  /* angle between robot orientation and goal middle */
  float angle;			
  
  VWDriveStraight(vw, 0.2, LIN_SPEED / 4.0);
  
  /* look for goal */
  VWGetPosition(vw, &pos);
  angle = DiscAtan((int)(1000 * pos.y), (int)(1000.0 * (LENGTH - pos.x))) + pos.phi;
  if(( (fabs(angle) < PI2) && (pos.x < WIDTH2)) || /* facing opponent's goal */
    ( (fabs(angle) < PI / 4) && (pos.x > WIDTH2)) )
  {
    VWDriveStraight(vw, 0.2, LIN_SPEED);
    kick_ball();
    got_ball_flag = FALSE;
    see_ball_flag = FALSE;
  }
  else			/* not facing opponent's goal */
  {
/*    move_camera(UP); */
    if (find_goal(&x_middle, &y_middle, &goal_size)) /* seeing opponent's goal */
    {
      LCDSetPos(1, 10);
      LCDPrintf("GOAL!\n");
      LCDSetPos(2, 10);
      LCDPrintf("     \n");
      LCDSetPos(3, 10);
      LCDPrintf("     \n");
      
      kick_ball();
      got_ball_flag = FALSE;
      see_ball_flag = FALSE;
    }
    else
			/* turn to opponent's goal */
			VWSetSpeed(vw, LIN_SPEED / 4.0, -fsign(angle) * LIN_SPEED); 
    /*    move_camera(DOWN);*/	/* make sure ball is still there */
  }
}




/***********************************************************************/
/** Turn and drive back to home position.
    
    Called by PPdrive_field().
    @see PPdrive_field()
*/
/***********************************************************************/

void drive_home()
{
  /* difference in coordinates from robot to ball */
  float diff_x, diff_y;     
  /* distance and angle from robot to ball */
  float home_angle, home_dist;
  
  /* speed depending on distance of home position */
  float drive_speed;

  PositionType pos;
  VWGetPosition(vw, &pos);

  diff_x = home[player_pos-2][next_home].x - pos.x;	/* difference to home position */
  diff_y = home[player_pos-2][next_home].y - pos.y;
  
  /* angle between robot orientation and line from robot middle
     through home position */
  home_angle = get_angle(diff_x, diff_y, pos.phi);
  
  /* distance to home position in a straight line */
  home_dist = hypot(diff_x, diff_y);

  if (start_flag)		/* just starting to go -> take
				   shortest way */
    direction = (int) home_angle;
  
  if (fsign(home_angle) == direction)
    VWDriveTurn(vw, home_angle, ROT_SPEED);
  else
    VWDriveTurn(vw, direction * 2.0 * PI + home_angle, ROT_SPEED);
  
  while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)
    OSReschedule();

  start_flag = FALSE;
  
  if (VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)
  {
    drive_speed = get_drive_speed(home_dist);    
    VWDriveStraight(vw, home_dist, drive_speed); /* drive home */
    while (!VWDriveDone(vw) && !see_ball_flag && !end_flag && !obstacle_flag)    
      OSReschedule();
    if (see_ball_flag)
				drive_stop();
    if (VWDriveDone(vw))
    {
	next_home++;
	if (next_home > 2)
	  next_home = 0;
    }
  }
}


/***********************************************************************/
/** 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;
  int ball_count = 0;
  int new_ball = FALSE;
  int done = FALSE;			/* flag to show whether robot
													  succesfully drove around ball */
  
  float angle_after_turn;	/* heading if robot would drive curve
														  to the ball */
  float ball_angle;		/* angle to the ball */
  
  float middle_x, middle_y, diff_x, diff_y, mid_dist, side;
  
  while(!end_flag)
  {
    /*     LCDPrintf("control\n"); */
    
    if (start_flag1 && attack_flag && !sim_motors)
    {
			if (attack_flag == 1)
			  OSWait(100);
			if((player_pos == 4) || (player_pos ==5))
			{
				VWDriveStraight(vw, 0.2, 1.5 * LIN_SPEED);
				while (!VWDriveDone(vw)) 
				{
					kick_ball();
					OSWait(50);
				}
			}
	
			if((player_pos == 2) || (player_pos == 3))
			{
				kick_ball();
				VWDriveStraight(vw, -0.1, 1.5 * LIN_SPEED);
				while (!VWDriveDone(vw)) {}
			}
			start_flag1 = FALSE;
    }
    
    /* double check if ball is in front of robot 
			(robot might have moved since taking the picture)*/
    if (see_ball_flag)
    {
			VWGetPosition(vw, &pos);
			if (hypot(ball.x - pos.x, ball.y - pos.y) < GOT_BALL)
			{
				/*	  move_camera(DOWN); */
				got_ball_flag = TRUE;
			}
    }
    
    LCDSetPos(0, 10);
    LCDPrintf("o%ds%dg%d\n", obstacle_flag, see_ball_flag, got_ball_flag);
    
    if (obstacle_flag)		/* ran into obtstacle -> avoid it */
    {
			if (!sim_motors)
			{
				obstacle_flag = FALSE;
				avoid_obstacle();
			}
    }
    else
			if (got_ball_flag) /* if ball is caught -> drive it to goal */
			{
				new_ball = FALSE;
				if (!sim_motors)
					drive_to_goal();
				if (USE_AUDIO)
				{
					AUBeep(); AUBeep(); AUBeep(); AUBeep(); AUBeep();
				}
			}
			else
				if (see_ball_flag && ball_count < LOOK_AT_BALL_CYCLES)
				{
					VWGetPosition(vw, &pos); /* robot position */
					get_ball_coord(&ball); /* ball position */

					new_ball = FALSE;
					see_ball_flag = FALSE; /* driving to current ball position
																 -> don't consider it again */
					if (!sim_motors)
					{
						if ((((ball.x < home[player_pos-2][0].x + LENGTH4) && /* ball outside region? */
							(ball.x > home[player_pos-2][0].x - LENGTH4))) ||
							((player_pos > 3) && fabs(pos.phi) < PI/8)) /* striker facing opponent's goal */
						{
							ball_count = 0;
							
							/* angle to ball */
							ball_angle = get_angle(ball.x - pos.x, ball.y - pos.y, pos.phi);
							
							/* angle in which robot would hit the ball after turning to it */
							angle_after_turn = pos.phi - 2 * ball_angle;
							if (fabs(angle_after_turn) > PI)
								angle_after_turn -= 2 * fsign(angle_after_turn) * PI;
							
							/* drive in/turn to right position to drive curve to ball */
							if (fabs(angle_after_turn) > PI2) /* turn to own goal */
							{
								LCDSetPos(4, 12);
								LCDPrintf("be1\n");
								
								/* side of ball where robot has to drive */
								diff_y = ball.y - pos.y;
								side = fsign(diff_y);
								
								ball.x -= arounddist; /* drive behind ball */
								
											/* position of middle of circle: used to check whether robot is too
												close to ball */
								middle_x = ball.x - side * arounddist / 2 * sin(ball.phi);
								middle_y = ball.y - side * arounddist / 2 * cos(ball.phi);
								
								/* difference of position robot - middle of circle */
								diff_x = middle_x - pos.x;
								diff_y = middle_y - pos.y;
								
								mid_dist = hypot(diff_x, diff_y);
								
								done = FALSE;
								if(mid_dist > arounddist)
									done = drive_around(diff_x, diff_y, (int) side, pos.phi);
								else
									VWDriveTurn(vw, side * PI, ROT_SPEED);
								
								if (done)
								{
									VWGetPosition(vw, &pos);
									/* angle to ball */
									ball_angle = get_angle(ball.x - pos.x, ball.y - pos.y, pos.phi);
									
									/* angle in which robot would hit the ball after turning to it */
									angle_after_turn = pos.phi - 2 * ball_angle;
									if (fabs(angle_after_turn) > PI)
										angle_after_turn -= 2 * fsign(angle_after_turn) * PI;
									
									LCDSetPos(4, 12);
									LCDPrintf("be2\n");
									
									if (fabs(angle_after_turn) < PI / 4)
									{
										start_curve_to_ball(pos, ball, TRUE); /* drive behind ball */
										
						/*			if (VWDriveDone(vw) && !see_ball_flag)
										move_camera(DOWN);  lost ball - maybe already in
													front of robot -> check */
									}
									else
										VWDriveTurn(vw, -side * PI, ROT_SPEED);
								}
								ball.x += arounddist; /* real ball position */
							}
							else
							{
								LCDSetPos(4, 12);
								LCDPrintf("frn\n");
								VWGetPosition(vw, &pos);
								start_curve_to_ball(pos, ball, FALSE); /* drive behind ball */
							}
						}
						else
							ball_count ++;
					} /* if (sim_motors) */
				}
				else /* (if see ball) no ball in sight */
				{
					ball_count = 0;
    			if (!sim_motors)
					{
				drive_stop();
							
						/* if (cam_pos == UP) 
						{*/
						/* drive_home(); */
						/*	    move_camera(DOWN); */
						/*	  } */
					}
					new_ball = see_ball_flag;
				}
				OSReschedule();	    
  }
  end_flag += 1;
  OSKill(0);
}
  
  

/***********************************************************************/
/** GOALIE: Drive to ball on circle near goal line.
    Drive curve near goal line while always facing the ball.
        
    Called by PPdrive_goal().

    @see PPdrive_goal()
    @param ball structure containing coordinates of ball and
    angle from ball position to opponent's goal
*/
/***********************************************************************/

void start_goal_line(PositionType ball)
{
  float robot_angle, diff_angle, correct_angle;
  float drive_speed;
  
  PositionType pos;
  
  VWGetPosition(vw, &pos);
  ball.phi = atan2(ball.y, goaldist + ball.x); /* ball angle */
  robot_angle = atan2(pos.y, goaldist + pos.x); /* goal angle */

  /* angle between lines middle of circle to robot and middle of
     circle to ball */
  diff_angle = ball.phi - robot_angle;
  
  see_ball_flag = FALSE;        /* already driving to current ball position */

  /* ------------------------ correct heading ------------------------- */
  /* correct heading if robot is standing at the corner of the goal
     and has to drive away: turn until robot is standing perpendicular
     to line to middle of circle */
  if ((fsign(diff_angle) != fsign(pos.y)) && (fabs(pos.y) >= GOALWIDTH / 3.0))
    VWDriveTurn(vw, -PI2 - pos.phi + robot_angle, ROT_SPEED); 
  while (!VWDriveDone(vw) && !see_ball_flag && !end_flag)
    OSReschedule();
  /* ------------------------------------------------------------------ */

  if (!end_flag && !obstacle_flag)
  {
    if (fabs(ball.phi) > anglemax)
      diff_angle = fsign(ball.phi) * anglemax - robot_angle;

    /* ------------------------ drive to ball ------------------------ */
    if ((fabs(diff_angle) > PI / 100.0)) /* don't react on little changes or if 
					 already facing ball */
    {
      drive_speed = get_drive_speed(fabs(diff_angle));  
      VWDriveCurve(vw, -diff_angle * radius, -fabs(diff_angle), drive_speed);  
      while (!VWDriveDone(vw) && 
             ((fsign(diff_angle) != fsign(pos.y)) || 
              (fabs(pos.y) < GOALWIDTH / 3.0)) && 
             !see_ball_flag && !obstacle_flag && !end_flag)
      {
        OSReschedule();
        VWGetPosition(vw, &pos);
      }
    }
		drive_stop();

    if (!see_ball_flag)
    {
      /* ------------------------ correct heading ------------------------- */
      /* turn to look at ball in case robot is standing at the corner
	 of its goal */
      correct_angle = PI2 + pos.phi - ball.phi;
      if (fabs(correct_angle) > PI / 200.0)
        VWDriveTurn(vw, -correct_angle, ROT_SPEED / 2.0); 
    }
  }
}

    
/***********************************************************************/
/** 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, last, pos;
  int last_flag = FALSE; /* flag whether last time ball has been detected */
  int factor = 0;
  
  while(!end_flag)
  {
    /*     LCDPrintf("control\n"); */
    
    LCDSetPos(0, 10);
    LCDPrintf("o%ds%d\n", obstacle_flag, see_ball_flag);
    
    if (obstacle_flag)
    {
			last_flag = FALSE;
			if (!sim_motors)
				drive_stop();
    }
    else 
			if (see_ball_flag)
			{  
				get_ball_coord(&ball);
				
				factor = 3;		/* allow factor pictures without ball 
													after driving to it */
				
				if ((ball.x < 0.3) || last_flag) /* use last ball coordinates
																						if available */
				{
					if (last_flag)	/* propable future ball position */
					{
						ball.x -= last.x - ball.x;
						ball.y -= last.y - ball.y;
					}
					VWGetPosition(vw, &pos);
					if (hypot(ball.x - pos.x, ball.y - pos.y) < GOT_BALL)
						kick_ball();    
					if (!sim_motors)
						start_goal_line(ball);
				}
				last = ball;
				last_flag = TRUE;
			}
			else                      /* no ball in sight */
			{
				last_flag = FALSE;
				if (factor)
					factor --;
				else
				{
					ball.x = 0.1;
					ball.y = 0.0;
					if (!sim_motors)
					{
						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);
}
  
/*@}*/