float TURNRATIO=-0.25;

int dstx, dsty, go_forward=1;

int speed() {
  if (go_forward)
    return MAX_SPEED;
  else
    return -MAX_SPEED;
  /*
  float d = distance(rf_x_us(), rf_y_us(), dstx, dsty);
  return (int)((float)MAX_SPEED * d/(float)MAXDIST);
  */
}

float crossprod(int vx0, int vy0, int vx1, int vy1) {
  return (float)vx0*(float)vy1 - (float)vx1*(float)vy0;
}

int cpsign(int vx0, int vy0, int vx1, int vy1) {
  float cp = crossprod(vx0, vy0, vx1, vy1);
  if (cp < (float)0)
    return -1;
  return 1;
}                                                                                
float mag(int x, int y) {
  return distance(0, 0, x, y);
}

float cosine(int vx0, int vy0, int vx1, int vy1) {
  /* float cp = crossprod(vx0, vy0, vx1, vy1); */
  float dp = (float)vx0*(float)vx1 + (float)vy0*(float)vy1;
  dp /= mag(vx0, vy0) * mag(vx1, vy1);
  return dp;
}

void set_new_dest(float x, float y) {
  dstx = (int)(x * (float)UNITSPERFT);
  dstx = (int)(y * (float)UNITSPERFT);
}

void move() {
  int oldx, oldy, xvel, yvel;
  int vel, lvel, rvel, dir, timeout;
  long time;

  vel = speed();
  lvel = vel;
  rvel = vel;
  while (1) {
    set_motor_power(lvel, rvel);
    printf("\nMoving forward  L=%d R=%d", lvel, rvel);

    oldx = rf_x_us(); oldy = rf_y_us();
    time = mseconds();
    timeout = 0;
    /* Move until a new reading or a timeout */
    while (eqwtol(oldx,rf_x_us()) && eqwtol(oldy,rf_y_us())) {
      if (mseconds() - time > 1500L) {
        timeout = 1;
        break;
      }
    }
    
    if (timeout == 1) {
      printf("\nStuck! Back up.");
      /* back up */
      set_motor_power(-MAX_SPEED, -MAX_SPEED);
      threadsafe_sleep(0.75);
      /* turn around */
      time = mseconds();
      while (eqwtol(oldx,rf_x_us()) && eqwtol(oldy,rf_y_us()) &&
             mseconds() - time < 1000L)
        ;
      xvel = oldx - rf_x_us();
      yvel = oldy - rf_y_us();
      dir = cpsign(xvel, yvel, dstx-rf_x_us(), dsty-rf_y_us());
      /* dir == -1: go right. dir == 1: go left. */
      set_motor_power(-MAX_SPEED * dir, MAX_SPEED * dir);
      threadsafe_sleep(0.5);
      alloff();
      vel = speed();
      lvel = vel;
      rvel = vel;
    } else {
      xvel = rf_x_us() - oldx;
      yvel = rf_y_us() - oldy;
      if (cosine(xvel, yvel, dstx-rf_x_us(), dsty-rf_y_us()) < COS10DEG) {
        /* We're more than 10 degrees off. Turn. */
        dir = cpsign(xvel, yvel, dstx-rf_x_us(), dsty-rf_y_us());
        vel = speed();
        /* dir == -1: go right. dir == 1: go left. */
        if (dir == -1) {
          lvel = vel;
          rvel = (int)((float)vel * TURNRATIO);
        } else {
          lvel = (int)((float)vel * TURNRATIO);
          rvel = vel;
        }
      } else {
        /* We're on the right track. */
        vel = speed();
        lvel = vel;
        rvel = vel;
      }
    }
  }
}

void balance_config() {
  float factor = 0.95; int last_change=-1;

  if (lmul < 1.0 - TOLERANCE && rmul < 1.0-TOLERANCE) {
    lmul = 1.0; rmul = 0.8;
  }

  printf("\nSTART=y STOP=n  Rebalance?\n");
  while (start_button() || stop_button())
    ;
  while (!start_button())
    if (stop_button())
      return;

  lmul = 1.0; rmul = 1.0;

  while (1) {
    printf("\n(l,r) = (%f,%f)\n", lmul, rmul);
    while (start_button() || stop_button())
      ;
    while (!start_button())
      if (stop_button())
	return;

    set_motor_power(25, 25);
    threadsafe_sleep(2.0);
    set_motor_power(0, 0);
    printf("\nSTART=y STOP=n  Adjust?\n");
    while (start_button() || stop_button())
      ;
    while (!start_button())
      if (stop_button())
	return;
    printf("\nSTART=L STOP=R\n");
    while (start_button() || stop_button())
      ;
    while (!stop_button() && !start_button())
      ;
    if (start_button()) {
      /* Lean back to the right */
      if (last_change == 1)
	factor = sqrt(factor);
      if (lmul >=  1.0-TOLERANCE)
	rmul *= factor;
      else
	lmul /= factor;
      last_change = 0;
    } else {
      if (last_change == 0)
	factor = sqrt(factor);
      /* Lean back to the left */
      if (rmul >=  1.0-TOLERANCE)
	lmul *= factor;
      else
	rmul /= factor;
      last_change = 1;
    }

    if (lmul < rmul && rmul < 1.0-TOLERANCE) {
      lmul = lmul / rmul;
      rmul = 1.0;
    } else if (rmul < lmul && lmul < 1.0-TOLERANCE) {
      rmul = rmul / lmul;
      lmul = 1.0;
    }
  }
}

int moving = 0;
void detect_motion() {
  int lastx = rf_x_us(), lasty = rf_y_us();

  while (1) {
    threadsafe_sleep(0.75);
    if (distance(lastx, lasty, rf_x_us(), rf_y_us()) < 10.0)
      moving = 0;
    else
      moving = 1;
    lastx = rf_x_us(), lasty = rf_y_us();
  }
}