#define TURND 0
#define TURN 0
#define ONRIGHT 10
#define TOORIGHT 11
#define ONLEFT 12
#define TOOLEFT 13
#define BACK 3

#define drive(spd) motor(3, spd);
#define hitright() digital(7)
#define hitleft() digital(9)

int redhori[5][4] = {{1, 1, 1, 1},
		     {0, 1, 0, 0},
		     {1, 0, 1, 0},
		     {1, 0, 0, 0},
		     {0, 0, 1, 0}};
int redvert[4][5] = {{0, 0, 0, 0, 1},
		     {1, 0, 1, 0, 1},
		     {0, 0, 0, 1, 1},
		     {0, 1, 1, 0, 1}};
int blackhori[5][4] = {{1, 1, 1, 1},
		       {0, 0, 0, 1},
		       {0, 1, 0, 0},
		       {0, 0, 0, 1},
		       {1, 1, 1, 1}};
int blackvert[4][5] = {{1, 0, 1, 0, 0},
		       {1, 1, 0, 0, 1},
		       {1, 1, 0, 1, 0},
		       {1, 0, 1, 0, 0}};
int whitehori[5][4] = {{0, 0, 1, 0},
		       {0, 1, 0, 0},
		       {1, 0, 0, 1},
		       {0, 1, 1, 0},
		       {1, 1, 1, 1}};
int whitevert[4][5] = {{1, 1, 0, 1, 1},
		       {1, 0, 0, 0, 1},
		       {1, 0, 1, 0, 1},
		       {1, 0, 0, 0, 1}};

int center;
int speed;
int x, y, *walls;

void turnto(float ang) {
  int new = center + (int) ((ang / 45.) * 100.);
  if (analog(TURN) < new) {
    fd(TURND);
    while (analog(TURN) < new && (analog(TURN) >= 5) && (analog(TURN) <= 250));
    off(TURND);
  } else {
    bk(TURND);
    while (analog(TURN) > new && (analog(TURN) >= 5) && (analog(TURN) <= 250));
    off(TURND);
  }
}  

void main() {
  center = analog(TURN);
  speed = 100;
  if (stop_button())
    ao();
  while (!start_button());
  set_ir_transmit_frequency(100);
  ir_transmit_on();
  bouncearound();
}

/* goal = 200 */
void followleft() {
  drive(speed);
  while (!hitleft() && !hitright()) {
    if (!digital(ONLEFT)) {
      turnto(10.);
      msleep(10L);
      turnto(0.);
    }
    if (digital(TOOLEFT)) {
      turnto(-10.);
      msleep(10L);
      turnto(0.);
    }
  }
  drive(0);
}

void followright() {
  drive(speed);
  while (!hitleft() && !hitright()) {
    if (!digital(ONRIGHT)) {
      turnto(-10.);
      msleep(10L);
      turnto(0.);
    }
    if (digital(TOORIGHT)) {
      turnto(10.);
      msleep(10L);
      turnto(0.);
    }
  }
  drive(0);
}

void bouncearound() {
  int i, n;
  while (1) {
    drive(speed);
    while (!hitright() && !hitleft()) {
      if (digital(ONLEFT) || digital(TOOLEFT))
	followleft();
      else if (digital(ONRIGHT) || digital(TOORIGHT))
	followright();
    }
    drive(0);
    if (hitright() && !hitleft()) {
      if (random(4))
	turnto(30.);
      else
	turnto(-30.);
    }
    if (hitleft()) {
      if (random(4))
	turnto(-30.);
      else
	turnto(30.);
    }
    drive(-speed);
    n = random(40);
    for (i = 0; i < n; i++) {
      if (analog(BACK) > 127) {
	drive(0);
	break;
      }
      sleep(.1);
    }
    turnto(0.);
  }
}