/*
 * Author : Mahdi Asadpour <asadpur@ce.sharif.edu>
 * File : soccer_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;
short move, max, counter;

#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 > 250)?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
        
//Image processing procedures
#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, ir0, ir1;
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);

  ir0 = robot_get_device("ir0");
  ir1 = robot_get_device("ir1");
  distance_sensor_enable(ir0, STEPTIME);
  distance_sensor_enable(ir1, STEPTIME);

  counter = 0;
  srand(time(NULL));
  move = rand()%3;
  max = 50;//50
  speed = 50;
  intouch = FALSE;
//  printf("reset done\n");
}

static int run(int ms) 
{
  short left_speed, right_speed;
  unsigned short ir0_value, ir1_value;
  int i, j;
  
  camimg = (unsigned char *)camera_get_image(camera);

  counter++;
  if (counter < max) {
    switch (move) {
    case 0:
      ir0_value = distance_sensor_get_value(ir0);
      ir1_value = distance_sensor_get_value(ir1);
      //printf("ir0=%d, ir1=%d\n", ir0_value, ir1_value);
      left_speed = right_speed = speed;
      if (ir1_value > 500 || ir0_value > 500) {
        left_speed  = -30;
        right_speed =  30;
      } 
      break;
    case 1:
     left_speed = -20;
     right_speed = 20;  
     for (j=0; j < height; j++)
       for (i=width/2-5; i < width/2+5; i++) {        
         P = get_pixel(camimg, i, j);
         //ball
         if (BALL(R(P),G(P),B(P))) {
           printf("(%d,%d), red=%d, green=%d, blue=%d\n",i, j,R(P),G(P),B(P));
           left_speed = right_speed = 50;
           goto next1; //break;
         }    
       }    
      next1:
      break;
    case 2:
      left_speed = -20;
      right_speed = 20;
      for (j=0; j < height; j++)
        for (i=width/2-1; i < width/2+1; i++) {        
          P = get_pixel(camimg, i, j);
          //blue goal
          if (BLUE_GOAL(R(P),G(P),B(P))) {
            printf("(%d,%d), red=%d, green=%d, blue=%d\n",i, j,R(P),G(P),B(P));
            left_speed = right_speed = 50;
            break;
          }    
        }    
      break;
    }//switch
  }
  
  if (counter >= max) {
    counter=0; 
    srand(time(NULL));
    max = 30+rand()/(RAND_MAX/70); /* random number between 30 and 99 */
    srand(time(NULL));
    move = rand()%3;
  }
  
  differential_wheels_set_speed(left_speed/2, right_speed/2);

  return STEPTIME;
}

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