/***********************************************************************/
/** @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();
 	LCDClear();
} 


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


/** factor to calculate y position from pixels to meters depending on
    x position (achieved by getting width in m of object that has
    width of screen at selected y positions. fact = val(m)/imagerows) */

float yfact[imagerows] =

/* camPos=MIDDLE (angle 150) */
{0.0106, 0.0093, 0.0080, 0.0069, 0.0065, 0.0061, 0.0058, 0.0054,
  0.0050, 0.0048, 0.0045, 0.0043, 0.0040, 0.0039, 0.0039, 0.0038,
  0.0037, 0.0036, 0.0036, 0.0035, 0.0035, 0.0034, 0.0034, 0.0033,
  0.0033, 0.0032, 0.0031, 0.0031, 0.0030, 0.0030, 0.0029, 0.0029,
  0.0028, 0.0028, 0.0027, 0.0027, 0.0026, 0.0026, 0.0025, 0.0024,
  0.0024, 0.0023, 0.0023, 0.0023, 0.0022, 0.0022, 0.0022, 0.0021,
  0.0021, 0.0020, 0.0020, 0.0020, 0.0019, 0.0019, 0.0019, 0.0019,
  0.0018, 0.0018, 0.0018, 0.0018, 0.0017, 0.0017};

  /* camPos=UP (angle 225) */
/* {0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124,
  0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124, 0.0124,
  0.0124, 0.0124, 0.0124, 0.0124, 0.0110, 0.0100, 0.0090, 0.0083,
  0.0075, 0.0068, 0.0060, 0.0057, 0.0055, 0.0053, 0.0051, 0.0050,
  0.0049, 0.0048, 0.0046, 0.0045, 0.0044, 0.0043, 0.0041, 0.0040,
  0.0039, 0.0038, 0.0037, 0.0036, 0.0035, 0.0034, 0.0034, 0.0033,
  0.0033, 0.0032, 0.0031, 0.0031, 0.0030, 0.0029, 0.0029, 0.0028,
  0.0027, 0.0027, 0.0026, 0.0025, 0.0024, 0.0023};
 
  /* camPos=DOWN (angle 0) */
/* {0.0026, 0.0026, 0.0026, 0.0026, 0.0025, 0.0025, 0.0025, 0.0025,
  0.0024, 0.0024, 0.0024, 0.0023, 0.0023, 0.0023, 0.0023, 0.0022,
  0.0022, 0.0022, 0.0022, 0.0021, 0.0021, 0.0021, 0.0021, 0.0021,
  0.0020, 0.0020, 0.0019, 0.0019, 0.0019, 0.0019, 0.0018, 0.0018,
  0.0018, 0.0018, 0.0017, 0.0017, 0.0017, 0.0017, 0.0016, 0.0016,
  0.0016, 0.0015, 0.0015, 0.0015, 0.0015, 0.0014, 0.0000, 0.0000,
  0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000,
  0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000};


/** factor to get x position in meters from x position in pixels */
float x2m[imagerows] =

 /* camPos=MIDDLE (angle 150) */
/*{0.520, 0.460, 0.400, 0.340, 0.320, 0.300, 0.280, 0.260, 0.240,
  0.227, 0.215, 0.202, 0.190, 0.182, 0.174, 0.166, 0.158, 0.150,
  0.145, 0.140, 0.135, 0.130, 0.125, 0.121, 0.117, 0.113, 0.109,
  0.105, 0.101, 0.097, 0.094, 0.090, 0.087, 0.084, 0.081, 0.078,
  0.076, 0.073, 0.071, 0.068, 0.066, 0.063, 0.061, 0.059, 0.057,
  0.055, 0.053, 0.051, 0.049, 0.047, 0.045, 0.042, 0.040, 0.037,
  0.035, 0.033, 0.031, 0.029, 0.027, 0.025, 0.023, 0.021};
 
 /* camPos=UP (angle 255) */
 {0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.990,
  0.990, 0.990, 0.990, 0.990, 0.990, 0.990, 0.900, 0.810, 0.750,
  0.690, 0.630, 0.570, 0.510, 0.450, 0.412, 0.375, 0.337, 0.300,
  0.287, 0.274, 0.261, 0.248, 0.235, 0.226, 0.217, 0.208, 0.200,
  0.192, 0.184, 0.176, 0.168, 0.160, 0.155, 0.150, 0.145, 0.140,
  0.135, 0.131, 0.127, 0.123, 0.119, 0.115, 0.111, 0.107, 0.103,
  0.100, 0.096, 0.092, 0.088, 0.084, 0.080, 0.076, 0.072};
 
/* camPos=DOWN (angle 0) */
 /*{0.095, 0.095, 0.092, 0.090, 0.087, 0.084, 0.081, 0.078, 0.075,
  0.072, 0.070, 0.067, 0.065, 0.063, 0.061, 0.059, 0.057, 0.055,
  0.053, 0.051, 0.049, 0.047, 0.045, 0.042, 0.040, 0.037, 0.035,
  0.033, 0.031, 0.029, 0.027, 0.025, 0.023, 0.021, 0.019, 0.017,
  0.015, 0.013, 0.012, 0.011, 0.010, 0.009, 0.008, 0.007, 0.006,
  0.005, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000,
  0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000};


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


/*@}*/