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

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <device/robot.h>
#include <device/custom_robot.h>
#include <device/servo.h>

static int width, height;
unsigned char *camimg;
static DeviceTag dcamera, rwheel, lwheel, 
                 rrwheel, llwheel, kicker,
                 emitter, receiver;
static char team;
static char player;
static float left_speed, right_speed;
unsigned char *P;
FILE *output;
int step, t;
//Emitter&Receiver Buffers
char *emitbuff;
const char *recvbuff;
char recvbufftmp[3];

//Define Colors
#define TRUE  1
#define FALSE 0
#define BALL(R, G, B)  (R > 245 && G < 10 && B < 10)?TRUE:FALSE
#define YELLOW_GOAL(R, G, B)	(R > 245 && G > 245 && B < 10)?TRUE:FALSE
#define BLUE_GOAL(R, G, B)	(R < 10 && G < 10 && B > 245)?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        
        
#define STEPTIME 64

// Defines image processing procedures
#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)

//State Machine : States
#define START				 		0
#define FIND_BALL 	 		1
#define GOTO_BALL 	 		2
#define ARVAND					3
#define FIND_OPPONENT_GOAL 	4
#define GOTO_OPPONENT_GOAL 	5
#define KICK_BALL_TO_GOAL 	6
#define FINISH 				7
#define DEFEND				8
#define OFFEND				9
#define NEAR_BLUE_GOAL 		10
#define NEAR_YELLOW_GOAL 	11
#define STOP 					12

//Messages
#define MSG_FIND   '0'
#define MSG_OWN    '1'
#define MSG_DED    '2'
#define MSG_OFD    '3'

// Implementation
static void reset(void) {
  const char *name;
  name     = robot_get_name();
  team     = name[0];
  player   = name[1];
    
  dcamera = robot_get_device("camera");
  camera_enable(dcamera, STEPTIME);   
  width = camera_get_width(dcamera); 
  height = camera_get_height(dcamera);

  rwheel = robot_get_device("rwheel");
  lwheel = robot_get_device("lwheel");
  servo_enable_position(rwheel, STEPTIME);
  servo_enable_position(lwheel, STEPTIME);

  rrwheel = robot_get_device("rrwheel");
  llwheel = robot_get_device("llwheel");
  servo_enable_position(rrwheel, STEPTIME);
  servo_enable_position(llwheel, STEPTIME);

  kicker = robot_get_device("kicker");
  servo_enable_position(kicker, STEPTIME);

  emitter = robot_get_device("emitter");
  receiver = robot_get_device("receiver");
  receiver_enable(receiver, STEPTIME);
  
  emitbuff = (char *)emitter_get_buffer(emitter);
  memcpy(emitbuff, "000", 3*sizeof(char));
  emitter_send(emitbuff, 3*sizeof(char));

  recvbufftmp[0]='0';
  recvbufftmp[1]='0';
  recvbufftmp[2]='0';
  
  step = START;  
  t = 0;
  output = fopen("test.out", "w");
}

static int run(int ms) 
{
  static float f1 = 0.0, f2 = 0.0;
  int n, i, j;
  char msg;
  
  camimg = (unsigned char *)camera_get_image(dcamera);
  // Update the receiver buffer 
  n = receiver_get_buffer_size(receiver);
  if (n) {
    recvbuff = (const char *)receiver_get_buffer(receiver);
    recvbufftmp[0] = recvbuff[0];
    recvbufftmp[1] = recvbuff[1];
    recvbufftmp[2] = recvbuff[2];
  }  
  // Each player checks his message 
  switch(player) {
  case '1': msg = recvbufftmp[0]; break;
  case '2': msg = recvbufftmp[1]; break;
  case '3': msg = recvbufftmp[2]; break;
  }  
  // Check
  switch (msg) {
  case MSG_FIND:
    if (step > FINISH)
      step = START;
    break;
  case MSG_OWN: //Player has the ball
    break;
  case MSG_DED: //Another player has the ball
    if (step < DEFEND)
      step = DEFEND;
    break;
  case MSG_OFD:
    if (step < OFFEND)
      step = OFFEND;
    break;
  default:
    //printf("Start...-> <%d>\n", msg);
    step = START;
  	break;      
  }    
  
  // State Machine
  switch (step) {
  // Start 
  case START:
      //printf("Start...\n");
      left_speed = right_speed = 0.0;
      if (t > 3*STEPTIME) {
        t = 0;
        step = FIND_BALL;
      }   
      else
        t += STEPTIME;
      break;
  // Find the ball
  case FIND_BALL:
     //printf("Find ball...\n");
     left_speed = 0.2;
     right_speed = -0.2;  
     for (j=0; j < height; j++)
       for (i=0; i < width; 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));
           if (i > width/2-5) {
             left_speed = 0.2;
             right_speed = -0.2;
           } else if (i < width/2-5) {
             left_speed = -0.2;
             right_speed = 0.2;
           } else
             left_speed = right_speed = 0.0;

           step = GOTO_BALL;
           goto next1; //break;
         }    
       }    
      next1:
      break;
  // Go to the ball 
  case GOTO_BALL:
     left_speed = 0.2;
     right_speed = 0.2;
     for (j=0; j < height; j++)
       for (i=width/2-1; i < width/2+1; i++) {        
         P = get_pixel(camimg, i, j);
         //ball
         if (BALL(R(P),G(P),B(P))) {
           left_speed = right_speed = 0.7;
           if (j > 5*height/9) {//ball is in touch
             step = ARVAND;
             // Send message to teamates
             emitbuff = (char *)emitter_get_buffer(emitter);
             switch (player) {
             case '1':
                 memcpy(emitbuff, "123", 3*sizeof(char));
                 break;
             case '2':
                 memcpy(emitbuff, "213", 3*sizeof(char));
                 break;
             case '3':
                 memcpy(emitbuff, "231", 3*sizeof(char));
                 break;
             }    
             emitter_send(emitter, 3*sizeof(char));
             left_speed = right_speed = 0.0;
           }    
           goto next2; //break;
         }    
       }
      //Where is the ball!
      emitbuff = (char *)emitter_get_buffer(emitter);
      memcpy(emitbuff, "000", 3*sizeof(char));
      emitter_send(emitter, 3*sizeof(char));
      step = START;
      next2:
      break;
  // Arvand mechanism 
  case ARVAND:
      servo_set_position(llwheel, 1.0);//-a1
      servo_set_position(rrwheel, -1.0);//-a2
      left_speed = -0.2;//v1
      right_speed = 0.2;//v2
      step = FIND_OPPONENT_GOAL;
      break;
  // Find opponent goal
  case FIND_OPPONENT_GOAL:
      left_speed = -0.2;//v1
      right_speed = 0.2;//v2
      for (j=0; j < height; j++)
        for (i=width/2-1; i < width/2+1; i++) {        
          P = get_pixel(camimg, i, j);
          //yellow goal
          if (YELLOW_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 = 0.0;
            servo_set_position(llwheel, 0.0);
            servo_set_position(rrwheel, 0.0);
            step = GOTO_OPPONENT_GOAL;
            goto next3; //break;
          }    
        }    
      next3:
      
      //check for the ball again
      for (j=0; j < height; j++)
        for (i=0; i < width; i++) {        
          P = get_pixel(camimg, i, j);
          //ball
          if (BALL(R(P),G(P),B(P)))
            goto next4; //break;
      	}
     	//the ball is not seen!
	    servo_set_position(llwheel, 0.0);
      servo_set_position(rrwheel, 0.0);
      emitbuff = (char *)emitter_get_buffer(emitter);
      memcpy(emitbuff, "000", 3*sizeof(char));
      emitter_send(emitter, 3*sizeof(char));
      step = START;
      
      next4:
      break;
  // Go to opponent goal 
  case GOTO_OPPONENT_GOAL:
      left_speed = right_speed = 0.3;//0.5
      if (t > 30*STEPTIME) {
        t = 0;
        step = KICK_BALL_TO_GOAL;
      }
      else 
        t += STEPTIME;
      break;
  // Kick ball to goal 
  case KICK_BALL_TO_GOAL:
      if (t > 5*STEPTIME) {
        servo_set_position(kicker, 1.5);
        t = 0;
        step = FINISH;
      }
      else {
      	servo_set_position(kicker, 6.0);
      	left_speed = right_speed = 0.5;
      	t += STEPTIME;
      }    
      break;
  // Finish 
  case FINISH:
      emitbuff = (char *)emitter_get_buffer(emitter);
      memcpy(emitbuff, "000", 3*sizeof(char));
      emitter_send(emitter, 3*sizeof(char));
    
      left_speed = right_speed = 0.0;
      step = START; // for infinite loop
      break;
  // Offend 
  case OFFEND:
      left_speed = 0.2;
      right_speed = -0.2;
      for (j=0; j < height; j++)
        for (i=width/2-5; i < width/2+5; i++) {        
          P = get_pixel(camimg, i, j);
          //yellow goal
          if (YELLOW_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 = 0.5;
            step = NEAR_YELLOW_GOAL;
            break;
          }    
        }    
      break;
  // Near yellow goal 
  case NEAR_YELLOW_GOAL:
     left_speed = 0.4;
     right_speed = 0.4;
     for (j=height/3; j < height/2; j++)
       for (i=0; i < width; i++) {        
         P = get_pixel(camimg, i, j);
         //yellow goal
         if (YELLOW_GOAL(R(P),G(P),B(P))) {
           printf("NEAR_YELLOW_GOAL\n");
           left_speed = right_speed = 0.0;
           step = STOP;
           goto next5; //break;
         }    
       }
      next5:
      break;
  // Defend
  case DEFEND:
      //step = STOP; break;//for testing
      left_speed = 0.2;
      right_speed = -0.2;
      for (j=0; j < height; j++)
        for (i=width/2-5; i < width/2+5; 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 = 0.4;
            step = NEAR_BLUE_GOAL;
            break;
          }    
        }    
      break;
  // Near blue goal
  case NEAR_BLUE_GOAL:
     left_speed = 0.4;
     right_speed = 0.4;
     for (j=height/3; j < height/2; j++)
       for (i=0; i < width; i++) {        
         P = get_pixel(camimg, i, j);
         //blue goal
         if (BLUE_GOAL(R(P),G(P),B(P))) {
           printf("NEAR_BLUE_GOAL\n");
           left_speed = right_speed = 0.0;
           step = STOP;
           goto next6; //break;
         }
       }
      next6:
      break;
  // Stop 
  case STOP:
      left_speed = right_speed = 0.0;
      break;
  }    

  f1 += left_speed;
  servo_set_position(lwheel, f1/2);
  f2 += right_speed;
  servo_set_position(rwheel, f2/2);

  return STEPTIME;
}

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