/*
 * Author : Mahdi Asadpour <asadpur@ce.sharif.edu>
 * File : goalie_yellow.c
 * Date : 02/2005
*/

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <device/robot.h>
#include <device/differential_wheels.h>
#include <device/receiver.h>
#include <device/distance_sensor.h>

static int width, height;
unsigned char *camimg;
static int speed;
unsigned char *P;
static float f;
unsigned char intouch;

#define GOAL_TIME_LIMIT 	1500
#define GOAL_Z_LIMIT 			0.25
#define COFF_TRAN					1.28

//Define Colors
#define TRUE  1
#define FALSE 0
#define BALL(R, G, B)  (R >= 250 && G < 20 && B < 5)?TRUE:FALSE
#define YELLOW_GOAL(R, G, B)	(R > 250 && G > 250 && B < 5)?TRUE:FALSE
#define BLUE_GOAL(R, G, B)	(R < 5 && G < 5 && B > 255)?TRUE:FALSE
#define BLUE_CORNER(R, G, B)	\\
        (R < 85&&R > 75&&G < 85&&G > 75&&B < 185&&B > 175)?TRUE:FALSE
#define YELLOW_CORNER(R, G, B)	\\
        (R < 185&&R > 175&&G < 185&&G > 175&&B < 85&&B > 75)?TRUE:FALSE
#define BLUE_ROBOT(R, G, B)	\\
        (R < 5 && G > 250 && B > 250)?TRUE:FALSE
#define YELLOW_ROBOT(R, G, B)	\\
        (R > 250 && G < 225 && G > 215 && B < 135 && B > 125)?TRUE:FALSE
        
//Defines
#define STEPTIME 64
#define get_pixel(camimg, x, y) camimg+3*(x+y*width)
#define set_pixel(camimg, x, y, z) camimg[3*(x+y*width)] = z
#define R(P) *(P)
#define	G(P) *(P+1)
#define B(P) *(P+2)

static DeviceTag camera;
static char team;
static char player;

static void reset(void) 
{
  const char *name;
  name     = robot_get_name();
  team     = name[0];
  player   = name[1];

  camera = robot_get_device("camera");  
  camera_enable(camera, STEPTIME);
  width = camera_get_width(camera); 
  height = camera_get_height(camera);

  speed = 50;
  intouch = FALSE;
//  printf("reset done\n");
}

static int run(int ms) 
{

  int i, j;
  
  camimg = (unsigned char *)camera_get_image(camera);

  intouch = FALSE;
  
  for (j=0; j < height; j++)
    for (i=0; i < width; i++) {        
      P = get_pixel(camimg, i, j);
      if (BALL(R(P),G(P),B(P))) {
        if (i > width/2-5)
          speed = -40;
        else if (i < width/2-5)
          speed = 40;
        else
          speed = 0;
        intouch = TRUE;
        goto seen;  
      }    
    }    

  if (speed == 0) {
    speed = 50;
    if (f > GOAL_TIME_LIMIT)
      speed = 50;
    else if (f < -GOAL_TIME_LIMIT)
      speed = -50;
  }  

  seen:

  if (f > GOAL_TIME_LIMIT || f < -GOAL_TIME_LIMIT) {
    if (intouch)
      speed = 0;
    else
      speed *= -1;
  }  

  if (speed > 0)
    f += STEPTIME;
  else if (speed < 0)
    f -= STEPTIME;
    
  differential_wheels_set_speed(speed/2, speed/2);

  return STEPTIME;
}

int main() 
{
  robot_live(reset);
  robot_run(run);
  return 0;
}