void threadsafe_msleep(long t) {
   long start = mseconds();
   while(mseconds()-start < t)
     defer();
}

void threadsafe_sleep(float t) {
   threadsafe_msleep((long)(t * 1000.0));
}

#define START 1
#define STOP 2
void button(int which) {
  /* Clear the start/stop button, then wait for a new press */
  if (which == START) {
    while (start_button())
      defer();
    while (!start_button())
      defer();
  } else if (which == STOP) {
    while (stop_button())
      defer();
    while (!stop_button())
      defer();
  }
}

/* Use the knob to configure some runtime parameter. */
int config(char name[], int min, int max) {
  int knobval, oldoutp = max+1, outp;
  while (start_button())
    ;
  while (!start_button()) {
    knobval = knob();
    outp = (int)(((float)knobval / 255.0) * (float)(max-min)) + min;
    if (outp != oldoutp) {
      oldoutp = outp;
      printf("\n%s = %d", name, outp);
    }
  }
  return outp;
}

/* Floating-point absolute value function */
float fabs(float a) {
  if (a < 0.0)
    a *= -1.0;
  return a;
}

int sign(float a) {
  if (a > 0.0) return 1;
  else return -1;
}

int isign(int a) {
  if (a > 0) return 1;
  else return -1;
}

/* Return the distance between two points. */
float distance(int x0, int y0, int x1, int y1) {
  float dx = (float)(x0 - x1), dy = (float)(y0 - y1);
  return (float)sqrt(dx*dx + dy*dy);
}

/* Test equality with an error tolerance */
int TOL = 10;
int eqwtol(int a, int b) {
  if (a-b < TOL && b-a < TOL)
    return 1;
  return 0;
}

/* Test equality with tolerance for floats */
float TOLERANCE = 1E-4;
int equal(float a, float b) {
  if (a-b < TOLERANCE && b-a < TOLERANCE)
    return 1;
  return 0;
}

/* Round floats to ints */
int round (float a) {
  return (int)(a + 0.5);
}

void set_motor_power(int leftpwr, int rightpwr) {
  motor(LEFT1, round(lmul*(float)(LFORWARD*leftpwr)));
  motor(RIGHT1, round(rmul*(float)(RFORWARD*rightpwr)));
  motor(LEFT2, round(lmul*(float)(LFORWARD*leftpwr)));
  motor(RIGHT2, round(rmul*(float)(RFORWARD*rightpwr)));
}

int our_num;

int rf_x_us() {
  if (our_num == 0)
    return rf_x0;
  else
    return rf_x1;
}
int rf_y_us() {
  if (our_num == 0)
    return rf_y0;
  else
    return rf_y1;
}
int rf_x_them() {
  if (our_num == 1)
    return rf_x0;
  else
    return rf_x1;
}
int rf_y_them() {
  if (our_num == 1)
    return rf_y0;
  else
    return rf_y1;
}