/* Functions
   gethackers
   makecurvedturn
   forwarduntilbumper
   backwarduntilbumper
   tryforwardintowall
*/

void gethackers(int direction, int power) {
  int ground0check = 0;
  int ground1check = 0;
  int black=0;
  
  alignstraight();
  backarmup();
  backwarduntilbumper(direction,50);
  frontarmup();
  forward(power);
  /*
    if (direction == 1)
    forwardslightleft(power);
    else
    forwardslightright(power);
  */

  whileencoders(1,150);
  ao();
  forwarduntilbumper(0,40);
  backarmup();

  if (direction == 1)
    backwardslightleft(power);
  else
    backwardslightright(power); 

  /*  backward(power); */

/* groundlight check for putting hackers in jail */
  reset_encoders();

  if (direction==1) {
    while (black<35) {
      if (ground0>200)
	black++;
      else
	black=0;
    }
  }
  else
    while (black<35) {
      if (ground1>200)
	black++;
      else
	black=0;
    }

  while(encoder(1) < 300 || encoder(2) < 300)
    ;
  /*  whileencoders(2,300); */
  ao();
  pause();
}

void getprofessor(int direction, int power) {

  int black=0;
  int blackcounter=0;
  int flag = 0;  

/* added by changa 1/27/00, 3am */
int groundlight = 0;
/* **************************** */

  frontarmup();
  backwarduntilbumper(0,power);
  pause();
  ao();
  if (direction == 1)
    wallturn90(2,60);
  else
    wallturn90(1,60);
  ao();
  pause();

  if (direction == 1)
    forwardslightright(power);
  else
    forwardslightleft(power);
  
  
  /*groundlight check for pulling professors back */
  /* reading for 77 Mass Ave */
  /* 50 7 6 8 31 */
  
  reset_encoders();

  while(encoder(1) < 100 || encoder(2) < 100)
    ;

  while(blackcounter<2) {
    if (direction == 1)
      groundlight = ground1;
    else
      groundlight = ground0;

    if (groundlight > (groundwhite + groundjail)/2) {
      black++;
      if (black > 25 && flag == 0) {
	flag = 1;
	blackcounter++;
      }
    }
    else {
      flag = 0;
    }
  }
  
  while(encoder(1) < 1000 || encoder(2) < 1000)
    ;
  
  ao();
  msleep(200L);
  backarmup();
  /*  if (direction == 1)
      backwardslightright(power);
      else
      backwardslightleft(power);
      */
  backward(power);
  whileencoders(2,800);
  
}

/*   direction = 0 : straight
*/

void forwarduntilbumper(int direction, int power) {
  int flag = 0; /*to prevent continuous forward(40) calls */

  reset_encoders();
  if (direction == 0)
    forward(power);
  else if (direction == 1)
    forwardslightleft(power);
  else
    forwardslightright(power);
  while(!bumpfl || !bumpfr) {
    if ((bumpfl && !bumpfr) || (!bumpfl && bumpfr)) {
      if (flag == 0)
	++flag;
      else {
	backabit();
	forward(60);
      }
      msleep(1000L);
    }
  }
  printf("front: %d %d",encoder(1),encoder(2));
  ao();
}


/* slows down after going over hacker
   direction = 0 : straight
   direction = 1 : left
   direction = 2: right */

void backwarduntilbumper(int direction, int power) {
  int flag = 0; /*to try the first time to continue realign */
  
  reset_encoders();
  if (direction == 0)
    backward(power);
  else if (direction == 1)
    backwardslightleft(power);
  else
    backwardslightright(power);  

  while(!bumpbl || !bumpbr) {
    if ((bumpbl && !bumpbr) || (!bumpbl && bumpbr)) {
      if (flag == 0)
	flag++;
      else {
	forwardabit();
	backward(60);
      }
      msleep(1000L);
    }
  }
  printf("back: %d %d\n",encoder(1),encoder(2));
  ao();
}

/* Back up just a little */
void backabit() {
  reset_encoders();
  backslow();
  whileencoders(2,70);
}

/* Forward just a little */
void forwardabit() {
  reset_encoders();
  forward(50);
  whileencoders(1,70);
  ao();
}


/* direction 1 = left, 2 = right */
int tryforwardintowall(int direction) {
  if (direction == 1) {
    forwardcurve(-40,80,90,-45);
    msleep(200L);
    ao();
    return bumpfr;
  }
  else {
    forwardcurve(80,-40,45,90);
    msleep(200L);
    ao();
    return bumpfl;
  }
}

void makecurvedturn(int direction) {
  reset_encoders();
  if (direction == 1) {
    forwardcurve(80,75,15,15);
    msleep(1000L);
    ao();
  }
  else {
    forwardcurve(70,90,-15,-15);
    msleep(1000L);
    ao();
  }
  printf("left: %d right: %d\n",encoder(1),encoder(2));
  waitforenter();
}


void ram(int direction) {
  if (direction == 1) {
    backward(60);
    msleep(500L);
    ao();
    msleep(200L);
    forward(100);
    msleep(1000L);
    forward(80);
  }
  else if (direction == 2) {
    forward(60);
    msleep(500L);
    ao();
    msleep(200L);
    backward(100);
    msleep(800L);
    backward(80);
  }

}

/*State 3 = going right, 4 = left */
void readjust(int state) {
  if (state == 3) {
    turnleft(50);
    msleep(200L);
    turnright(50);
    msleep(200L);
  }
  else   if (state == 4) {
    turnright(50);
    msleep(200L);
    turnleft(50);
    msleep(200L);
  }

}