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

#include <device/robot.h>
#include <device/supervisor.h>
#include <device/emitter.h>
#include <stdio.h>
#include <string.h>

// Team names
char BLUE_TEAM[10] = "arvand01";
char YELLOW_TEAM[10] = "arvand02";

#define STEPTIME 			640 
#define LOG_ENABLE  	0
#define ROBOTS 				8 // number of robots
#define GOAL_X_LIMIT	0.745
#define GOAL_Z_LIMIT 	0.2
#define OUT_X_LIMIT  	GOAL_X_LIMIT
#define OUT_Z_LIMIT  	0.65
#define AFTER_OUT 		0.1

#define TRUE  1
#define FALSE 0

// Global variables
FILE *logfile;
char goal; // which team got new score 
static const char *robot_name[ROBOTS]=
    {"B1","B2","B3","B4","Y1","Y2","Y3","Y4"};
static NodeRef     robot[ROBOTS];
static NodeRef     ball;
static float ball_reset_timer;

// use Position for LOG purposes
static float position[(ROBOTS*6+2)*sizeof(float)];
static float ball_initial_position[2]={0,0};
static float robot_initial_positionb[ROBOTS][6]={// Blue starts the game
    {0.10, 0, 0.0, 1.0, 0.0, 1.57},
    {0.38, 0.37, 0.0, 1.0, 0.0, 1.57},
    {0.38,-0.37, 0.0, 1.0, 0.0, 1.57},
    {0.75, 0, 0.0, 1.0, 0.0, 1.57},
    {-0.38, 0, 0.0, -1.0, 0.0, 1.57},
    {-0.38, 0.38, 0.0, -1.0, 0.0, 1.57},
    {-0.38,-0.38, 0.0, -1.0, 0.0, 1.57},
    {-0.75, 0, 0.0, -1.0, 0.0, 0},
  };

static float robot_initial_positiony[ROBOTS][6]={// Yellow starts the game
    {0.38, 0.005, 0.0, 1.0, 0.0, 1.57}, 
    {0.38, 0.37, 0.0, 1.0, 0.0, 1.57},
    {0.38,-0.37, 0.0, 1.0, 0.0, 1.57},
    {0.75, 0, 0.0, 1.0, 0.0, 1.57},
    {-0.10, 0, 0.0, -1.0, 0.0, 1.57}, 
    {-0.38, 0.38, 0.0, -1.0, 0.0, 1.57},
    {-0.38,-0.38, 0.0, -1.0, 0.0, 1.57},
    {-0.75, 0, 0.0, -1.0, 0.0, 0},
  };


// Import function: Imports the Robots Model 
// of each team to the soccer field.
static void import(void)
{
  int i;
  char imfile[20];
  // Select a team as the starter, randomly.
  srand(time(NULL));
  if (rand()%2 == 0) {
    char tmp[10];
    strcpy(tmp, BLUE_TEAM);
    strcpy(BLUE_TEAM, YELLOW_TEAM);
    strcpy(YELLOW_TEAM, tmp);
  }    
  
  // Imports robots model to soccer field.
  for (i = 1; i < 5; i++) {
    sprintf(imfile, "%s\b%d.wbt", BLUE_TEAM, i);
    supervisor_import_node(imfile, -1);
  }  
  for (i = 1; i < 5; i++) {
    sprintf(imfile, "%s\b%d.wbt", YELLOW_TEAM, i);
    supervisor_import_node(imfile, -1);
  }
  
  // Revert the game with new added objects.
  supervisor_simulation_revert();
}  

// Set_scores function: Sets the score received by each team.
static void set_scores(int y, int b) 
{
  char score[16];
  sprintf(score,"%d",b);
  supervisor_set_label(0,score,0.92,0.04,0.07,0x0000ff); // blue
  sprintf(score,"%d",y);
  supervisor_set_label(1,score,0.05,0.04,0.07,0xffff00); // yellow
}

// Reset function: Initiates the variables and etc.
static void reset(void) 
{
  int i;

  logfile = fopen("log.txt", "w");
  set_scores(0,0);

  //import();
  
  // Blue starts the game firstly
  for(i=0;i<ROBOTS;i++) {
    robot[i]=supervisor_node_get_from_def(robot_name[i]);  
   	supervisor_field_set(robot[i],
         SUPERVISOR_FIELD_TRANSLATION_X|
  		   SUPERVISOR_FIELD_TRANSLATION_Z|
         SUPERVISOR_FIELD_ROTATION_X| 
         SUPERVISOR_FIELD_ROTATION_Y|
         SUPERVISOR_FIELD_ROTATION_Z|
  		   SUPERVISOR_FIELD_ROTATION_ANGLE,
  		   robot_initial_positionb[i]);
  }
  
  ball=supervisor_node_get_from_def("BALL");  
  supervisor_field_set(ball,
         SUPERVISOR_FIELD_TRANSLATION_X|
         SUPERVISOR_FIELD_TRANSLATION_Z,
  	     ball_initial_position);

  // Update ball position
  supervisor_field_get(ball,
         SUPERVISOR_FIELD_TRANSLATION_X|
		     SUPERVISOR_FIELD_TRANSLATION_Z,
		     &position[ROBOTS*3], STEPTIME/10);
		     
  ball_reset_timer = 0;
}


// Run function: it is run endlessly by Webots.
static int run(int ms) 
{
  int i;
  static int score[2]={0,0};
  static float time=10*60; // a match lasts for 10 minutes
  char time_string[64];
  static float ball_after_out_position[] = {0};

  // Log the position of Ball and Robots
  if (LOG_ENABLE) {
    for(i=0; i < ROBOTS; i++) {
      supervisor_field_get(robot[i],
  			   SUPERVISOR_FIELD_TRANSLATION_X|
  			   SUPERVISOR_FIELD_TRANSLATION_Z|
           SUPERVISOR_FIELD_ROTATION_X| 
           SUPERVISOR_FIELD_ROTATION_Y|
           SUPERVISOR_FIELD_ROTATION_Z|
  			   SUPERVISOR_FIELD_ROTATION_ANGLE,
  			   &position[i*3], STEPTIME/10);
    }
    supervisor_field_get(ball,
           SUPERVISOR_FIELD_TRANSLATION_X|
  		     SUPERVISOR_FIELD_TRANSLATION_Z,
  		     &position[ROBOTS*3], STEPTIME/10);
  		     
    fwrite(position, sizeof(ROBOTS*3+2), 0, logfile);
  }    

  time -= (float)STEPTIME/1000;
  // finish the game
  if (time < 0) { 
    char str[20];
    FILE *result = fopen("result.out", "w");
    fprintf(result, "Result: Yellow(%s) Team (%d), Blue(%s) Team(%d)\n",
             YELLOW_TEAM, score[0], BLUE_TEAM, score[1]);
    if (score[0] == score[1]) {
      fprintf(result, "Two teams are equal!\n");
      sprintf(str, "Game Over! Two teams are equal!");
    } else {
      fprintf(result, "The winner is %s\n",
              score[0]>score[1]?YELLOW_TEAM:BLUE_TEAM);
      sprintf(str,"Game Over! The winner is <%s>",
              score[0]>score[1]?YELLOW_TEAM:BLUE_TEAM); 
    }    
    supervisor_set_label(3, str, 0.05, 0.4, 0.07, 0xff0000);
    fclose(result);
    if (time < -0.01*STEPTIME)
      supervisor_simulation_quit();
    return STEPTIME/10;
  }    
  
  sprintf(time_string,"%02d:%02d",(int)(time / 60),(int)time % 60);
  supervisor_set_label(2,time_string,0.45,0.04,0.07,0x000000); // black

  if (ball_reset_timer==0) {
    // Check the Y out line
    if (position[ROBOTS*3+1] > OUT_Z_LIMIT 
        || position[ROBOTS*3+1] < -OUT_Z_LIMIT) {
      ball_after_out_position[0] = OUT_Z_LIMIT - AFTER_OUT;
      if (position[ROBOTS*3+1] < -OUT_Z_LIMIT)
        ball_after_out_position[0] *= -1;
      supervisor_field_set(ball,
			   SUPERVISOR_FIELD_TRANSLATION_Z,
			   ball_after_out_position);
    } 
    
    // Set score, and check the X out line 
    if (position[ROBOTS*3] > OUT_X_LIMIT || position[ROBOTS*3] < -OUT_X_LIMIT) {
      // ball in the blue goal 
      if (position[ROBOTS*3] > GOAL_X_LIMIT 
          && position[ROBOTS*3+1] > -GOAL_Z_LIMIT
          && position[ROBOTS*3+1] < GOAL_Z_LIMIT ) {
        set_scores(++score[0],score[1]);
        goal = 'y';
        ball_reset_timer = (float)1/3; 
      } else 
      // ball in the yellow goal 
      if (position[ROBOTS*3]<-GOAL_X_LIMIT 
          && position[ROBOTS*3+1] > -GOAL_Z_LIMIT
          && position[ROBOTS*3+1] < GOAL_Z_LIMIT ) {
        ball_reset_timer = (float)1/3;
        set_scores(score[0],++score[1]);
        printf("set_score : y=%d, b=%d, timer=%f\n", score[0], 
                score[1], ball_reset_timer);
        goal = 'b';
      } else {
        // ball in Out state
        ball_after_out_position[0] = OUT_X_LIMIT - AFTER_OUT;
        if (position[ROBOTS*3] < -OUT_X_LIMIT)
          ball_after_out_position[0] *= -1;
        supervisor_field_set(ball,
			     SUPERVISOR_FIELD_TRANSLATION_X,
			     ball_after_out_position);
        }  
      }
  } else {
    ball_reset_timer -= (float)STEPTIME/10000;
    if (ball_reset_timer <= 0) {
      printf("timer=%f\n", ball_reset_timer);    
      ball_reset_timer=0;
      supervisor_field_set(ball,
			   SUPERVISOR_FIELD_TRANSLATION_X|
			   SUPERVISOR_FIELD_TRANSLATION_Z,
			   ball_initial_position);
      if (goal == 'y')
        for(i=0;i<ROBOTS;i++)
          supervisor_field_set(robot[i],
  	          SUPERVISOR_FIELD_TRANSLATION_X|
  			      SUPERVISOR_FIELD_TRANSLATION_Z|
              SUPERVISOR_FIELD_ROTATION_X| 
              SUPERVISOR_FIELD_ROTATION_Y|
              SUPERVISOR_FIELD_ROTATION_Z|
  			      SUPERVISOR_FIELD_ROTATION_ANGLE,
  			      robot_initial_positionb[i]);
    
      if (goal == 'b')
        for(i=0;i<ROBOTS;i++)
          supervisor_field_set(robot[i],
  	          SUPERVISOR_FIELD_TRANSLATION_X|
  			      SUPERVISOR_FIELD_TRANSLATION_Z|
              SUPERVISOR_FIELD_ROTATION_X| 
              SUPERVISOR_FIELD_ROTATION_Y|
              SUPERVISOR_FIELD_ROTATION_Z|
  			      SUPERVISOR_FIELD_ROTATION_ANGLE,
  			      robot_initial_positiony[i]);
    }
  }
  return STEPTIME/10;
}

// Main function
int main() 
{
  robot_live(reset);
  robot_run(run); 
  fclose(logfile);
  return 0;
}