int too_close=115;
int six_inches=110;
int twelve_inches=100;

int diff=10;
int irmin=90;

int left;
int right;
int center;
int bump;

int r_motor=0;
int l_motor=1;

float init_speed;
float final_speed;
float n=0.0;
float old_speed;
float new_speed;
float x;
float k=0.05;
float stopped=1.0;
float speed;

float incr_speed(float old){
  if (old == 0.0){
    old=1.0;
  }
  x=(1.0/(1.0-k)^(n+1.0));
  new_speed=x * old;
  n++;
  old_speed=new_speed;
  return new_speed;
}

void go_fwd (float old_speed, float final_speed)
{
  speed=incr_speed(old_speed);
  motor(r_motor,speed);
  motor(l_motor,speed);

  while(speed < final_speed){

    speed=incr_speed(old_speed);
      if (speed > final_speed){
        speed=final_speed;
      }
    motor(r_motor,speed);
    motor(l_motor,speed);
  }
  n=0.0;
}

float decr_speed(float old){
  x=((1.0-k)^(n+1.0));
  new_speed=x * old;
  n++;
  if (new_speed < 1.0)
    new_speed=0.0;
  old_speed=new_speed;
  return new_speed;
}

void go_back(float final_speed, int delay_time){
  if (old_speed == 0.0){
    old_speed = 1.0;
  }

  if (old_speed < 0.0){
    old_speed= -old_speed;
  }

  if (final_speed < 0.0){
    final_speed= -final_speed;
  }

  speed=incr_speed(old_speed);
  motor(r_motor,-speed);
  motor(l_motor,-speed);

  while(speed < final_speed){
    speed=incr_speed(old_speed);

      if (speed > final_speed){
        speed=final_speed;
      }

    motor(r_motor,-speed);
    motor(l_motor,-speed);        
  }
  n=0.0;
  wait(delay_time);
}

void wait (int milli_seconds)
{
  long timer_a;

  timer_a = mseconds() + (long) milli_seconds;
  while (timer_a > mseconds()) {
    defer();
  }
}

void read_sensors()
{
  right=analog(0);
  center=analog(1);
  left=analog(2);  
  bump=analog(3);
}

void turn_right(float final_speed, int delay_time)
{
  speed=incr_speed(old_speed);
  motor(r_motor,-speed);
  motor(l_motor,speed);

  while(speed < final_speed){
    speed=incr_speed(old_speed);

      if (speed > final_speed){
        speed=final_speed;
      }

    motor(r_motor,-speed);
    motor(l_motor,speed);
  }
  old_speed=speed;
  new_speed=speed;
  n=0.0;
  wait(delay_time);
}

void turn_left(float speed, int delay_time)
{
  speed=incr_speed(old_speed);
  motor(r_motor,speed);
  motor(l_motor,-speed);

  while(speed < final_speed){
    speed=incr_speed(old_speed);

      if (speed > final_speed){
        speed=final_speed;
      }

    motor(r_motor,speed);
    motor(l_motor,-speed);
  }
  old_speed=speed;
  new_speed=speed;
  n=0.0;
  wait(delay_time);
}

void halt (int delay_time)
{
  if (old_speed < 0.0){
    old_speed= -old_speed;
    speed=decr_speed(old_speed);
    motor(r_motor,-speed);
    motor(l_motor,-speed);

    while(speed != 0.0){

      speed=decr_speed(old_speed);
      motor(r_motor,-speed);
      motor(l_motor,-speed);
    }
  }/*end if speed < 0.0*/

  if (old_speed > 0.0){        
    speed=decr_speed(old_speed);
    motor(r_motor,speed);
    motor(l_motor,speed);

    while(speed != 0.0){

      speed=decr_speed(old_speed);
      motor(r_motor,speed);
      motor(l_motor,speed);
    }/*end while*/
  }/*end if speed > 0.0*/

n=0.0;
wait(delay_time);
}

void avoid()
{
  while(1){
    poke(0x7000,0xff);
    wait(1);
    read_sensors();

  if (bump!=0){
/*    if ((bump==7) || (bump==8) || (bump==1) || (bump==2)){
      halt(1);
      go_fwd(old_speed,100.0);
    }*/

    if (bump>=15){
      halt(1);
      go_back(-25.0,500);
      halt(1);
      turn_right(25.0,1000);
      go_fwd(old_speed,50.0);
    }
  }/*end if bump!=0*/

  if (bump==0){
    if (center < twelve_inches){

      if ( (left > irmin) && (left > right+diff) ){
        turn_right(100.0,0);
        go_fwd(old_speed, 50.0);
      }
      else

      if ( (right > irmin) && (right > left+diff) ){
        turn_left(100.0,0);
        go_fwd(old_speed, 50.0);
      } 
      else

      if ( (left < right+diff) && (left > right) && (right < left+diff) &&
       (right > left) ) {
          go_fwd(old_speed, 100.0);
      }
    } /* end if center greater than 12in */

    if ( (center >= twelve_inches) && (center < six_inches) ){

      if ( (left > irmin) && (left > right+diff) ){
        turn_right(50.0,0);
        go_fwd(old_speed,25.0);
      } 
      else

      if ( (right > irmin) && (right > left+diff) ){
        turn_left(50.0,0);
        go_fwd(old_speed,25.0);
      } 
      else

      if ( (left < right+diff) && (left > right) && (right < left+diff) &&
       (right > left) ) {
          go_fwd(old_speed,50.0);
      }
    } /* end if center btwn 6in and 12in */
        
    if (center >= six_inches && center < too_close){

      if ( (left > irmin) && (left > right+diff) ){
        turn_right(25.0,0);
        go_fwd(old_speed,15.0);
      } 
      else

      if ( (right > irmin) && (right > left+diff) ){
        turn_left(25.0,0);
        go_fwd(old_speed,15.0);
      } 
      else

      if ( (left < right+diff) && (left > right) && (right < left+diff) &&
       (right > left) ) {
          go_fwd(old_speed,25.0);
      }
    } /* end if center between too_close and 6in */
        

    if (center >= too_close){
      halt(50);
      go_back(-25.0,500);
      halt(50);
      turn_right(15.0,1000);
      go_fwd(old_speed,50.0);
    }/* end if center is too_close */
 
  }/*end if bump==0*/

  }/*end while(1)*/

}/*end avoid()*/
  
void main()
{   
  go_fwd(stopped,100.0);
  wait(250);
  start_process(avoid());
}