/***********************************************************************/
/** @name imageLib.c 
    Contains application specific image processing process.
		Uses LibVision library.
    
    @author Birgit Graf,   UWA, 1998, modified 2000 (Mk3/4)
*/
/***********************************************************************/

/*@{*/


#include "global.h"
#include "imageVis.h"
#include "LibVision.h"
#include "servos.h"
#include "driveglobal.h"


/* global variables */
VIS_RGBSpace RGBSpace;                /* User's RGB space */
BYTE         MY_COLOUR_CLASS=1;       /* This example handles only one class */
BYTE         FoundClass;              /* Colour class found */

/** Ball coordinates when ball has been detected (position on field) */
float ball_x, ball_y;


void Init_Camera()
{
  int error=VIS_InitCam(NORMAL);
  if (error==INITERROR) { VIS_ErrorDisplay("   Cam Init.", INITERROR, "");}

  /*- Fill the hue conversion table and initialise the algorithm defaults -*/
  VIS_Init();
  
  /*- Fill the user's RGB space with zeros -*/
  VIS_ColClear(&RGBSpace);

	/*- set ball colour -*/
	LCDClear();
	Set_BallColour();
	Set_CameraParams();
} 


void Set_BallColour()
{	
	/* Finding the median hue and filling the RGBSpace */
	VIS_ColInit(&RGBSpace, MY_COLOUR_CLASS);
	if (USE_AUDIO)
		AUBeep();
 	LCDClear();
}


void Set_CameraParams()
{	
	/* calibrate camera */
	VIS_CamCal(&RGBSpace, MY_COLOUR_CLASS);

	/* init distance table */
	VIS_InitDistance();
	if (USE_AUDIO)
		AUBeep();
 	LCDClear();
}


/***********************************************************************/
/** 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;

  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;
}


/***********************************************************************/
/** Get ball coordinates.
    Get position of ball on field as found out in image processing
    routines and orientation of ball towards goal.
    
    Called by PPdrive_field() and PPdrive_goal().

    @see PPdrive_field()
    @see PPdrive_goal()
    @param *ball pointer to array containing coordinates of ball
    @return ball position and orientation towards goal in referenced
    parameter 
*/
/***********************************************************************/

void get_ball_coord(PositionType *ball)
{
 ball->x = ball_x;
 ball->y = ball_y;
 ball->phi = -atan2(ball_y, LENGTH - ball_x); /* neg: angle ball to goal */
}


/***********************************************************************/
/** 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 */
	
	image					 greyimg;
	colimage     ColPic;                  /* User's colour image */
	VIS_Object   Object;                  /* User's object */
 	VIS_Distance ObjDist;									/* distance to 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;

    /* Get a picture */
    CAMGetColFrame(&ColPic, 0);
		IPColor2Grey(&ColPic, &greyimg);

    /* Find class */
    FoundClass=VIS_ColFind(&ColPic, &RGBSpace, &Object, MY_COLOUR_CLASS);

    if (FoundClass == MY_COLOUR_CLASS)
    {
			/* set ball parameters */
			x_middle = Object.bot;
			ball_size= (Object.right - Object.left);
			y_middle = Object.left  + ball_size/2;

			VIS_MarkObject(greyimg, x_middle, y_middle, ball_size);
			LCDPutGraphic(&greyimg);
     
			VIS_GetPosition(&Object, &ObjDist);

			/* ------ see ball -> set coordinates and see_ball_flag ------ */
      cam_local.x = (float)ObjDist.d_row / 1000; /* x position from robot in m */
      cam_local.y = -(float)ObjDist.d_col / 1000; /* y 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 */
	  
/*					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 = atan(rob_local.y/rob_local.x) * 180 / 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) */
/*      {
				LCDPutGraphic(&greyimg);
	
				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
			{
				LCDPutGraphic(&greyimg);

				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);
}


/*@}*/