/***********************************************************************/
/** @name image.c 
    Contains application specific image processing process.
    
    @author Birgit Graf,   UWA, 1998, modified 2000 (Mk3/4)
    @author Thomas Braunl, UWA, 1998 (HSV)
*/
/***********************************************************************/

/*@{*/



#include "image.h"
#include "imageglobal.c"
#include "servos.h"



/***********************************************************************/
/** Transform local to global Position.
    change local dx, dy from robot position into global coordinates

    @author  Thomas Braunl
*/
/***********************************************************************/

void local2global(PositionType pos, PositionType local, PositionType *global)
{
  float cosphi, sinphi;

  if (I_am_goalie())	/* change coordinates for goalie */
    pos.phi += PI / 2.0;

  cosphi = cos(pos.phi);
  sinphi = sin(pos.phi);

  global->x   = pos.x + cosphi*local.x + sinphi*local.y;
  global->y   = pos.y + cosphi*local.y + sinphi*local.x;
  global->phi = 0.0;
}



/***********************************************************************/
/** Check for ball.
    Analyse picture by calling find_object and checking camera
    position, set see_ball_flag and respectively.

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

void PPball_test()
{
  PositionType rob_pos, camera_pos;
  int x_middle = 0, y_middle = 0, ball_size = 0; /* parameters of ball */
  
  /* ball coordinates (camera's / robot's local/ global coord.syst.) */
  PositionType cam_local, rob_local, global;
  
  int factor = 0;                       /* counts pictures without ball */
  int frame_counter, new_counter;				/* to calculate frame rate */
  int new_angle;

  frame_counter = OSGetCount();
  while(!end_flag)
  {
/*    LCDPrintf("ball\n"); */
    
    new_counter = OSGetCount();
    LCDSetPos(5, 10);		/* print frame rate */
    if (new_counter != frame_counter) 
      LCDPrintf("f:%1.1f\n", 100.0 / (float)(new_counter - frame_counter));
    frame_counter = new_counter;

    got_ball_flag = FALSE;
    if (find_ball(&x_middle, &y_middle, &ball_size))
    {
      /* ------ see ball -> set coordinates and see_ball_flag ------ */
      cam_local.y = (float)(imagecolumns / 2 - 1 - y_middle) * yfact[x_middle]; 
			/* y position from robot in m */
      cam_local.x = x2m[x_middle]; /* x position from robot in m */
	
			camera_pos.x = 0; camera_pos.y = 0;
			camera_pos.phi = (float) cam_pos / 180 * PI;	/* camera orientation */
      local2global(camera_pos, cam_local, &rob_local); /* get local coordinates (consider camera pos)*/      
	
			get_pos (&rob_pos);
			local2global(rob_pos, rob_local, &global); /* get global coordinates */      
	
			ball_x = global.x;
			ball_y = global.y;

			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));
	  
     /* check whether ball is in field */
/*      if ((global.x > 0.0) && (global.x < LENGTH) && (fabs(global.y) < WIDTH2))
      {	
*/        see_ball_flag = TRUE;	/* really seeing ball -> set flag + coordinates */
	    
				if (USE_AUDIO)
					AUBeep();		  
	  
        factor = 3;		   /* allow factor pictures without ball
														before looking elsewhere */
	  
			  if (I_am_goalie())
				  kick_ball();  /* kick it away */
			  else
				{
/*					KEYWait(ANYKEY);
					LCDClear();
					new_angle = DiscAtan((int) (1000.0 * rob_local.y), (int) (1000.0 * rob_local.x))
					* 180 / PI;	not accurate enough!!! */
					new_angle = (int) ( atan(rob_local.y/rob_local.x) * 180.0 / PI );

/*					LCDPrintf("cx: %d,cy: %d\n", (int) (100 * cam_local.x), (int) (100 * cam_local.y));
					LCDPrintf("cam angle: %d\n", cam_pos);
					LCDPrintf("rx: %d,ry: %d\n", (int) (100 * rob_local.x), (int) (100 * rob_local.y));
					LCDPrintf("rob angle: %d\n", (int) (rob_pos.phi * 180 / PI));
					LCDPrintf("gx: %d,gy: %d\n", (int) (100 * global.x), (int) (100 * global.y));
					LCDPrintf("new angle: %d\n", new_angle);
					KEYWait(ANYKEY);
*/
					move_camera(new_angle);
/*					LCDClear();
    	    LCDMenu("", "", "STOP", "");
*/				}

				if (hypot(rob_local.x, rob_local.y) < GOT_BALL)
					got_ball_flag = TRUE;
/*			}
      else     no ball (found object out of field, propably yellow goal) */
/*      {
				LCDSetPos(1, 10);
				LCDPrintf("     \n");
				LCDSetPos(2, 10);
				LCDPrintf("     \n");
				LCDSetPos(3, 10);
				LCDPrintf("     \n");
				
				move_camera(0);
				see_ball_flag = FALSE;  lost ball */
/*			}
*/    }
    else                        /* no ball */
    {
			if (factor)               /* keep on looking to presumed ball position */
				factor  --;
			else
			{
				LCDSetPos(1, 10);
				LCDPrintf("     \n");
				LCDSetPos(2, 10);
				LCDPrintf("     \n");
				LCDSetPos(3, 10);
				LCDPrintf("     \n");
				
				move_camera(0);
				see_ball_flag = FALSE;  /* lost ball */
			}
    }
    OSReschedule();
  }
  end_flag += 1;
  OSKill(0);
}


/*@}*/