/*****************************************************************************/
/* Name: supervisor.c                                                          */
/* Description: Template for creating a new Webots controller.               */
/* Author: Olivier Michel <Olivier.Michel@xxxxxxxxxxx.xxx>                   
*/
/* Date: September 9th, 2004                                                 */
/*****************************************************************************/

/*****************************************************************************/
/* You may need to add include files like <device/distance_sensor.h> or      */
/* <device/differential_wheels.h>.                                           */
/*****************************************************************************/

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

#define XCOORD 0
#define ZCOORD 1
#define BATTERY 2
#define WAIT_TIME 500000000
#define DEBUG 1

/*****************************************************************************/
/* You should declare here DeviceTag global variables for storing pointers   */
/* robot devices.                                                            */
/*****************************************************************************/
static NodeRef robots[3];
static float dat[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
static int numtrapped = 0;
static int robotrapped[2] = {0,0};
static float loctrapped[2][2] = {{0,0},{0,0}};
static float trap[2][2] = {{0.05, -1.65}, {0.05, 1.75}};
static float full_battery[1] = {100};
static char trapped[3] = {0,0,0};

/*****************************************************************************/
/* This is the reset function called once upon initialization of the robot.  */
/*****************************************************************************/
static void reset(void) {
  /***************************************************************************/
  /* You should insert robot_get_device() functions to get pointers to robot */
  /* devices (DeviceTag) and use them in the main control loop.              */
  /***************************************************************************/
  robots[0] = supervisor_node_get_from_def("ROBOT_0");
  robots[1] = supervisor_node_get_from_def("ROBOT_1");
  robots[2] = supervisor_node_get_from_def("ROBOT_2");
}


/*****************************************************************************/
/* This is the main control loop function, it is called repeatedly by Webots */
/*****************************************************************************/
static int run(int ms) {
  int i;
  /***************************************************************************/
  /* Enter here functions to read sensor data like                           */
  /* distance_sensor_get_value().                                            */
  /***************************************************************************/
  for (i = 0; i < 3 ; ++i) {
    supervisor_field_get(robots[i],
  		       SUPERVISOR_FIELD_TRANSLATION_X |
  		       SUPERVISOR_FIELD_TRANSLATION_Z |
		       SUPERVISOR_FIELD_BATTERY_CURRENT,
  		       &dat[i],64);
  }

  /***************************************************************************/
  /* Process sensor data here.                                               */
  /***************************************************************************/
  for (i = 0; i < 3; ++i) {
    // Check whether robots[i] wins
    if (dat[i][XCOORD] < -1.39 &&
	dat[i][ZCOORD] < -1.39) {
      printf("Robot%i won\n",i);
      int j;
      for (j=0; j < WAIT_TIME; ++j) {
      }
      supervisor_simulation_quit();
    }
    // Check if robots[i]'s battery is depleted
    if ((dat[i][BATTERY] < 0.5) && (dat[i][BATTERY] > 0.2)) {
      // if at least another robot is still alive
      if (numtrapped < 2) {
	// If the robot is already trapped don't change its position
	// and keep the log  of its old position.
	if (!trapped[i]) {
	  trapped[i] = 1;
	  // copy current location of robot
	  loctrapped[numtrapped][0] = dat[i][0];
	  loctrapped[numtrapped][1] = dat[i][1];
	  // store which robot is trapped in which trap
	  robotrapped[numtrapped] = i;
	
	  supervisor_field_set(robots[i],
			       SUPERVISOR_FIELD_TRANSLATION_X |
			       SUPERVISOR_FIELD_TRANSLATION_Z,
			       &trap[numtrapped]);
	}
			        
	// In any case set its battery to full.
	supervisor_field_set(robots[i],
			     SUPERVISOR_FIELD_BATTERY_CURRENT,
			     &full_battery);
	++numtrapped;
      }
      else { // All three robots have died
	// Fill all batteries and return other two to original locations.
	numtrapped = 0;
	trapped[0] = 0;
	trapped[1] = 0;
	trapped[2] = 0;
	supervisor_field_set(robots[robotrapped[0]],
			     SUPERVISOR_FIELD_TRANSLATION_Z |
			     SUPERVISOR_FIELD_TRANSLATION_X,
			     &loctrapped[0]);
	supervisor_field_set(robots[robotrapped[0]],
			     SUPERVISOR_FIELD_BATTERY_CURRENT,
			     &full_battery);
	supervisor_field_set(robots[robotrapped[1]],
			     SUPERVISOR_FIELD_TRANSLATION_Z |
			     SUPERVISOR_FIELD_TRANSLATION_X,
			     &loctrapped[1]);
	supervisor_field_set(robots[robotrapped[1]],
			     SUPERVISOR_FIELD_BATTERY_CURRENT,
			     &full_battery);
	supervisor_field_set(robots[i],
			     SUPERVISOR_FIELD_BATTERY_CURRENT,
			     &full_battery);
      }
    }	
  }
  
  /***************************************************************************/
  /* Enter here functions to send actuator commands,                         */
  /* like differential_wheels_set_speed().                                   */
  /***************************************************************************/

  return 64; /* this is the time step value, expressed in milliseconds. */
}

/*****************************************************************************/
/* This is the main program which sets up the reset and run function.        */
/*****************************************************************************/
int main() {
  robot_live(reset); /* initialization */
  robot_run(run);    /* starts the controller */
  return 0;
}