#define ERROR_RG ((long) 500)
#define BIT_TIME 500
#define TIME_OUT ((long) 4000)
#define IR       7

float thisloc_x;
float thisloc_y;
float otherloc_x;
float otherloc_y;

void setup_robot(int which) {
  start_process(track_robot(which));
}

void track_robot(int which) {
  long last;
  int robs[4];
  int loc, j;

  while (!digital(IR));
  while (1) {
    /* Start */
    robs[0] = robs[1] = robs[2] = robs[3] = j = 0;
    last = mseconds();
    while (digital(IR));
    loc = addones(robs, j, 0, (int) (mseconds() - last) / BIT_TIME - 4);
    if (loc > 8) {
      j++;
      loc -= 8;
    }
    while (j < 4) {
      last = mseconds() - ERROR_RG;
      while (!digital(IR));
      loc += (int) (mseconds() - last) / BIT_TIME;
      if (loc > 8) {
	j++;
	loc -= 8;
      }
      if (!((int) (mseconds() - last) / BIT_TIME > 6) && j < 4) {
	last = mseconds() - ERROR_RG;
	while (digital(IR));
	loc = addones(robs, j, loc, (int) (mseconds() - last) / BIT_TIME);
	if (loc > 8) {
	  j++;
	  loc -= 8;
	}
      }
    }
   
    thisloc_x = (float) robs[which * 2];
    thisloc_y = (float) robs[which * 2 + 1];
    otherloc_x = (float) robs[(!which) * 2];
    otherloc_y = (float) robs[(!which) * 2 + 1];
  }
}

int addones(int data[], int j, int loc, int count) {
  int mask;
  int down;

  if (count > 0) {
    mask = 1 << loc;
    down = count;
    while (down--) {
      if (!loc) {
	j++;
	mask = 1;
      }
      data[j] |= mask;
      mask <<= 1;
    }
  }

  return loc + count;
}