/*
  LIGHT SENSORS
  *************

  Sensor#  blue   white   Port#  Location
  --------------------------------------------------
   6       30      8      19     middle left
   5       18      6      16     actual front left
   3       40     10      22     side right
   4       20      6      20     side left
   7       77/160 15/56   18     center
   1       14      4      23     middle right
   2       22      6      17     actual front right
  --------------------------------------------------
*/

#define LSP_MIDDLE_LEFT        19
#define LSP_MIDDLE_RIGHT       23
#define LSP_CENTER             18
#define LSP_SIDE_LEFT          20
#define LSP_SIDE_RIGHT         22
#define LSP_ACTUAL_FRONT_LEFT  16
#define LSP_ACTUAL_FRONT_RIGHT 17

/* #define LST_MIDDLE_LEFT        40
#define LST_MIDDLE_RIGHT       16
#define LST_CENTER            100
#define LST_SIDE_LEFT          23
#define LST_SIDE_RIGHT         43
#define LST_ACTUAL_FRONT_LEFT  32
#define LST_ACTUAL_FRONT_RIGHT 23 */

#define NSENSORS 7
#define LS_ACTUAL_FRONT_LEFT    0
#define LS_ACTUAL_FRONT_RIGHT   1
#define LS_CENTER               2
#define LS_MIDDLE_LEFT          3
#define LS_MIDDLE_RIGHT         4
#define LS_SIDE_LEFT            5
#define LS_SIDE_RIGHT           6

int lsports[NSENSORS]= { LSP_ACTUAL_FRONT_LEFT, LSP_ACTUAL_FRONT_RIGHT,
			 LSP_CENTER,
			 LSP_MIDDLE_LEFT, LSP_MIDDLE_RIGHT,
			 LSP_SIDE_LEFT, LSP_SIDE_RIGHT };

/*int lstresh[NSENSORS]= { LST_ACTUAL_FRONT_LEFT, LST_ACTUAL_FRONT_RIGHT,
			 LST_CENTER,
			 LST_MIDDLE_LEFT, LST_MIDDLE_RIGHT,
			 LST_SIDE_LEFT, LST_SIDE_RIGHT }; */

int lstresh[NSENSORS][2];
int majority;

#define MANUAL_ORIENT 0

#define SP_REAR_SIDE_LEFT       9
#define SP_ACTUAL_REAR_LEFT    10
#define SP_ACTUAL_REAR_RIGHT   11
#define SP_REAR_SIDE_RIGHT     12
#define SP_FRONT_SIDE_LEFT     15
#define SP_ACTUAL_FRONT_RIGHT  13
#define SP_ACTUAL_FRONT_LEFT   14
#define SP_FRONT_SIDE_RIGHT    24

#define SP_IR_FRONT 28
#define SP_IR_BACK  30
#define SP_IR_RIGHT 29
#define SP_IR_LEFT  31
#define IR_FRONT 1
#define IR_BACK  2
#define IR_RIGHT 3
#define IR_LEFT  4

#define LEFT_SERVO 5
#define RIGHT_SERVO 4

#define RIGHT_DIFFTURN 750
#define LEFT_DIFFTURN 3500

/*#define LEFT_CENTER 1930
  #define RIGHT_CENTER 2000 */

#define LEFT_CENTER 2240
#define RIGHT_CENTER 2000

#define FASTSPEED 100
#define FASTSPEED_CORR 65

#define SLOWSPEED 65
#define SLOWSPEED_CORR 40

#define LEFT_TURNED_LEFT 1240
#define RIGHT_TURNED_LEFT 1000

#define LEFT_TURNED_RIGHT 3240
#define RIGHT_TURNED_RIGHT 3000

#define RIGHT_MOTORS 0
#define LEFT_MOTORS 1

#define FAST 1
#define SLOW 0

#define LEFT 0
#define RIGHT 1
#define BOTH 2

#define NSPEEDS 2
int speeds[NSPEEDS]={50,100};

int ws_sensors[10];

void print_color(int color)
{
    if( color==0 )
        printf("WHITE");
    else if( color==1 )
        printf("BLUE");
    else printf("INVALID COLOR");
}

int calibrate()
{
    int i,j;
    for( i=0;i<2; i++ )
    {
        printf("Hit START to calibrate ");
        print_color(i);
        printf("...\n");
        start_press();
        for( j=0;j<NSENSORS;j++ )
        {
            lstresh[j][i]=analog(lsports[j]);
        }
    }
    sleep(0.5);
    beep();
    printf("Calibration DONE.\n");
}

int get_ir()
{
  int result=0;
  if( digital(SP_IR_FRONT) )
      return IR_FRONT;
  if( digital(SP_IR_BACK) )
      return IR_BACK;
  if( digital(SP_IR_RIGHT) )
      return IR_RIGHT;
  if( digital(SP_IR_LEFT) )
      return IR_LEFT;
  return result;
}

int read_light_sensor(int sensor)
{
    int val=analog(lsports[sensor]);
/*    printf("%d%d%d:",val,lstresh[sensor][0],lstresh[sensor][1]); */
    if( abs(val-lstresh[sensor][0])<abs(val-lstresh[sensor][1]) )
        return 0;
    else return 1;
}

void setspeed(int side, int speed)
{
    int i;
    for( i=2*side;i<2*side+2; i++ )
    {
        motor(i,speed);
    }
}

int walk_shaftenc_dist(int speed, int cspeed, int nrot, long timeo,int color)
{
    int i,slow,j,timedout;
    int succ;
    long starttime;
    long seensince;
    int shaftenc[2],shaftcount[2];
    for(i=0;i<2;i++ )
    {
	shaftenc[i]=digital(i+7);
	shaftcount[i]=0;
    }
    slow=-1;
    setspeed(0,speed);
    setspeed(1,speed);
    starttime=mseconds();
    seensince=-1L;
    j=0;
    timedout=0;
    for( ;; )
      {
          j++;
          if( color!=-1 )
          {
              succ=0;
              for( i=0; ws_sensors[i]; i++ )
                  if( read_light_sensor(ws_sensors[i]) ==color )
                      succ=1;
/*              if( succ==0 && seensince!=-1L && mseconds()>seensince+20L )
                  break; */
              if( !succ && seensince!=-1L )
                  seensince=-1L;
              if( succ && seensince==-1L )
                  seensince=mseconds();
              if( seensince!=-1L && mseconds()>seensince+10L )
                break; 
          }
	if( timeo!=0L && mseconds()>=starttime+timeo )
	  {
	    printf("TIMEOUT.\n");
	    timedout=1;
	    break;
	  }
	for( i=0;i<2;i++ )
	  if( digital(i+7) != shaftenc[i] )
	    {
	      shaftenc[i]=1-shaftenc[i];
	      shaftcount[i]++;
	      if( slow==-1 && shaftcount[i]>shaftcount[1-i]+2)
		{
		  slow=i;
		  setspeed(i,cspeed);
		} else
	      if( slow!=-1 )
		if ( shaftcount[slow]<shaftcount[1-slow]+1)
		{
		  setspeed(slow,speed);
		  slow=-1;
		}
/*	      printf("shaftenc0=%d shaftenc1=%d\n",shaftcount[0],shaftcount[1]); */
	      if( nrot && shaftcount[i]==nrot )
		break;
	    }
	if( i<2 )
	  break;
      }
    setspeed(0,0);
    setspeed(1,0);
    return timedout;
}

int walk_shaftenc_dist2(int speed, int cspeed, int nrot,long timeo)
{
    int i, slow, sign;
    int shaftenc[2],shaftcount[2];
    long starttime;
    int timedout;
    printf("Walking at speed %d for %d steps...\n",speed,nrot);
    for(i=0;i<2;i++ )
      {
	shaftenc[i]=digital(i+7);
	shaftcount[i]=0;
      }
    slow=-1;
    timedout=0;
    setspeed(0,speed);
    setspeed(1,-speed);
    starttime=mseconds();
    for( ;; )
      {
	if( timeo!=0L && mseconds()>=starttime+timeo )
	  {
	    printf("TIMEOUT.\n");
	    timedout=1;
	    break;
	  }
	for( i=0;i<2;i++ )
	  if( digital(i+7) != shaftenc[i] )
	    {
	      shaftenc[i]=1-shaftenc[i];
	      shaftcount[i]++;
	      if( slow==-1 && shaftcount[i]>shaftcount[1-i]+1)
		{
		  slow=i;
		  if( i==1 ) sign=-1; else sign=1;
		  setspeed(i,sign*cspeed);
		} else
	      if( slow!=-1 )
		if ( shaftcount[slow]<shaftcount[1-slow]+1)
		{
		  if( slow==1 ) sign=-1; else sign=1;
		  setspeed(slow,sign*speed);
		  slow=-1;
		}
/*	      printf("shaftenc0=%d shaftenc1=%d\n",shaftcount[0],shaftcount[1]); */
	      if( nrot && shaftcount[i]==nrot )
		break;
	    }
	if( i<2 )
	  break;
      }
    setspeed(0,0);
    setspeed(1,0);
    return timedout;
  }

int walk_till_hit(int speed,int right,int left,long timeo)
{
    int i,status;
    int shaftenc[2],shaftcount[2];
    long starttime;
    int timedout;
    printf("Walking at speed %d until hitting a wall...\n",speed);
    for(i=0;i<2;i++ )
    {
	shaftenc[i]=digital(i+7);
	shaftcount[i]=0;
    }
    setspeed(0,speed);
    setspeed(1,speed);
    status=3;
    timedout=0;
    starttime=mseconds();
    for( ;; )
    {
	if( timeo!=0L && mseconds()>=starttime+timeo )
	{
	  printf("TIMEOUT.\n");
	  timedout=1;
	  break;
	}
	if( digital(right) && (status&1) )
        {
	    setspeed(0,0);
	    status-=1;
        }
	if( digital(left) && (status&2) )
        {
	    setspeed(1,0);
	    status-=2;
        }
	if( !status )
	  break;
	for( i=0;i<2;i++ )
	  if( digital(i+7) != shaftenc[i] )
          {
	      shaftenc[i]=1-shaftenc[i];
	      shaftcount[i]++;
/*	      printf("shaftenc0=%d shaftenc1=%d\n",shaftcount[0],shaftcount[1]); */
          }
    }
    setspeed(0,0);
    setspeed(1,0);
    if( !timedout )
        return -1;
    if( status==2 )
        return LEFT;
    if( status==1 )
        return RIGHT;
    return BOTH;
}

int diff_turn(int speed, int cspeed,int shaftenc,long timeo)
{
  int timedout;
  servo(LEFT_SERVO, LEFT_DIFFTURN);
  servo(RIGHT_SERVO, RIGHT_DIFFTURN);
  sleep(.3);
  timedout=walk_shaftenc_dist2(speed,cspeed,shaftenc,timeo);
  servo(LEFT_SERVO, LEFT_CENTER);
  servo(RIGHT_SERVO, RIGHT_CENTER);
  sleep(.3);
  return timedout;
}

#define IPOS_STR    0
#define IPOS_BACK   1
#define IPOS_WALL   2
#define IPOS_CENTER 3

void print_pos(int pos)
{
    if( pos==IPOS_STR )
        printf("STRAIGHT");
    if( pos==IPOS_BACK )
        printf("BACK");
    if( pos==IPOS_WALL )
        printf("WALL");
    if( pos==IPOS_CENTER )
        printf("CENTER");    
}

/*  |      |opponent's
    |      |color
    |      +-----          ---> go this way
    |
    |  own color
    +------------
*/

int line_following(int speed,int nrot,int linecolor,long timeo)
{
    int i;
    int shaftenc[2],shaftcount[2];
    long finish;
    int cc;
    long starttime;
    int timedout;

    for(i=0;i<2;i++ )
    {
	shaftenc[i]=digital(i+7);
	shaftcount[i]=0;
    }
    setspeed(0,speed);
    setspeed(1,speed);
    finish=-1L;
    cc=0;
    timedout=0;
    starttime=mseconds();
    for( ;; )
    {
      if( timeo!=0L && mseconds()>=starttime+timeo )
	{
	  printf("TIMEOUT.\n");
	  timedout=1;
	  break;
	}
	for( i=0;i<2;i++ )
            if( digital(i+7) != shaftenc[i] )
	    {
                shaftenc[i]=1-shaftenc[i];
                shaftcount[i]++;
/*                printf("shaftenc0=%d shaftenc1=%d\n",shaftcount[0],shaftcount[1]); */
                if( nrot && shaftcount[i]==nrot )
                    break;
	    }
	if( i<2 )
            break;

        if( finish!=-1L&& finish<=mseconds() )
        {
            cc=0;
            finish=-1L;
            servo(LEFT_SERVO, LEFT_CENTER);
            servo(RIGHT_SERVO, RIGHT_CENTER);
        }


        if( cc!=1 && (read_light_sensor(LS_SIDE_LEFT)==linecolor ||
                      read_light_sensor(LS_MIDDLE_RIGHT)!=linecolor) )
        {
            servo(LEFT_SERVO,LEFT_CENTER-400);
            servo(RIGHT_SERVO,RIGHT_CENTER-400);
            cc=1;
        }
        if( cc!=2 && (read_light_sensor(LS_SIDE_RIGHT)==linecolor ||
                      read_light_sensor(LS_MIDDLE_LEFT)!=linecolor) )
        {
            servo(LEFT_SERVO,LEFT_CENTER+400);
            servo(RIGHT_SERVO,RIGHT_CENTER+400);
            cc=2;
        }
    }
    setspeed(0,0);
    setspeed(1,0);
    return timedout;
}

int wall_following(int speed,int nrot,int lasthit,long timeo)
{
    int i,hit;
    int shaftenc[2],shaftcount[2];
    long lastshaft;
    long starttime;
    int timedout;
    int cc;
    long finished;

    for(i=0;i<2;i++ )
    {
	shaftenc[i]=digital(i+7);
	shaftcount[i]=0;
    }
    setspeed(0,speed);
    setspeed(1,speed);
    timedout=0;
    lastshaft=starttime=mseconds();
    cc=-1;
    finished=0L;
    for( ;; )
    {
        if( timeo!=0L && mseconds()>=starttime+timeo )
	{
            printf("TIMEOUT.\n");
            timedout=1;
            break;
	}
        if( finished!=0L && mseconds()>=finished )
        {
            servo(LEFT_SERVO, LEFT_CENTER);
            servo(RIGHT_SERVO, RIGHT_CENTER);            
            finished=0L;
            cc=-1;
        }
	for( i=0;i<2;i++ )
            if( digital(i+7) != shaftenc[i] )
	    {
                lastshaft=mseconds();
                shaftenc[i]=1-shaftenc[i];
                shaftcount[i]++;
/*                printf("shaftenc0=%d shaftenc1=%d\n",shaftcount[0],shaftcount[1]); */
                if( nrot && shaftcount[i]==nrot )
                    break;
	    }
        if( cc!=1 && digital(SP_FRONT_SIDE_LEFT) )
        {
            servo(LEFT_SERVO, LEFT_CENTER+200);
            servo(RIGHT_SERVO, RIGHT_CENTER+200);            
            finished=mseconds()+300L;
            lasthit=LEFT;
            cc=1;
        }
        if( cc!=2 && digital(SP_FRONT_SIDE_RIGHT) )
        {
            servo(LEFT_SERVO, LEFT_CENTER-200);
            servo(RIGHT_SERVO, RIGHT_CENTER-200);            
            finished=mseconds()+300L;
            lasthit=RIGHT;
            cc=2;
        }
        hit=-1;
        if( cc!=1 && digital(SP_ACTUAL_FRONT_LEFT) )
        {
            hit=LEFT;
        }
        if( cc!=2 && digital(SP_ACTUAL_FRONT_RIGHT) )
        {
            hit=RIGHT;
        }
        if( mseconds()>lastshaft+250L )
        {
            hit=lasthit;
        }
        if( hit!=-1 )
        {
            lasthit=hit;
            if( cc!=-1 )
            {
                servo(LEFT_SERVO, LEFT_CENTER);
                servo(RIGHT_SERVO, RIGHT_CENTER);            
                finished=0L;
                cc=-1;
            }
            if( hit==LEFT )
            {
                walk_shaftenc_dist(-50,-30,0,400L,-1);
                diff_turn(-100,-85,6,0L);
                setspeed(0,speed);
                setspeed(1,speed);
            }
            if( hit==RIGHT )
            {
                walk_shaftenc_dist(-50,-30,0,400L,-1);
                diff_turn(100,85,6,0L);
                setspeed(0,speed);
                setspeed(1,speed);
            }
            lastshaft=mseconds();
        }
    }
    setspeed(0,0);
    setspeed(1,0);
    return timedout;
}

int timeout_walk_till_hit(int speed,int right,int left,long timeout,int trials)
{
  int trial;
  int result;
  for( trial=0; trial<trials; trial++ )
  {
      result=walk_till_hit(speed,right,left,timeout);
      if( result==-1 )
	break;
      walk_shaftenc_dist(-speed,-speed,0,250L,-1);
      if( result==LEFT )
      {
          diff_turn(-speed,-speed,4,0L);
      }
      if( result==RIGHT )
      {
          diff_turn(speed,speed,4,0L);
      }
  }
  return result;
}

void orient()
{
  int pos,fr,fl,br,bl,ir;
  int count[2];
  int val;

  if( MANUAL_ORIENT )
  {
      pos=0;
      printf("Select pos: ");
      print_pos(pos%4);
      printf(" ");
      print_color(pos/4);
      printf(".\n");
      
      for( ;; )
      {
          majority=-1;
          while (majority==-1 )
          {
              if( start_button() )
                  majority=0;
              if( stop_button() )
                  majority=1;
          }
          if( majority )
          {
              pos++;
              if( pos==8 )
                  pos=0;
          }
          else
          {
              majority=pos/4;
              pos=pos%4;
              break;
          }
          printf("Select pos: ");
          print_pos(pos%4);
          printf(" ");
          print_color(pos/4);
          printf(".\n");
          while( start_button() || stop_button() );
      }
  }
  else
  {
      fr=read_light_sensor(LS_ACTUAL_FRONT_RIGHT);
/*      printf("%d",fr); */
      fl=read_light_sensor(LS_ACTUAL_FRONT_LEFT);
/*      printf("%d",fl); */
      br=read_light_sensor(LS_SIDE_RIGHT);
/*      printf("%d",br); */
      bl=read_light_sensor(LS_SIDE_LEFT);
/*      printf("%d\n",bl); */

/*  printf("read: fr=%d fl=%d br=%d bl=%d\n",fr,fl,br,bl); 
      for( ;; ); */
  count[0]=count[1]=0;
  count[fr]++;
  count[fl]++;
  count[br]++;
  count[bl]++;
  if( count[0]!=1 && count[1]!= 1 )
  {
      majority=get_ir();
      pos=-1;
      if( majority==IR_FRONT )
          pos=IPOS_CENTER;
      if( majority==IR_BACK )
          pos=IPOS_WALL;
      if( majority==IR_LEFT )
          pos=IPOS_STR;
      if( majority==IR_RIGHT )
          pos=IPOS_BACK;
      if( pos==-1 )
          for( ;; )
          {
              printf("COULD NOT ORIENT!!!\n");
              sleep(3.0);
              printf("read: fr=%d fl=%d br=%d bl=%d ir=%d\n",fr,fl,br,bl,majority);
              sleep(5.0);
          }
      else {
          majority=-1;
          beep();
          beep();
          beep();
      }
  } else
  {
      if( count[0]==1 ) majority=1; else majority=0;
  
      if( fr != majority ) pos=IPOS_CENTER;
      if( fl != majority ) pos=IPOS_STR;
      if( br != majority ) pos=IPOS_BACK;
      if( bl != majority ) pos=IPOS_WALL;

  }
  }

  printf("Starting position is ");
  print_pos(pos);
  printf(".\n");

  enable_servos();
  servo(LEFT_SERVO, LEFT_CENTER);
  servo(RIGHT_SERVO, RIGHT_CENTER);
  ws_sensors[0]=LS_ACTUAL_FRONT_LEFT;
  ws_sensors[1]=LS_ACTUAL_FRONT_RIGHT;
  ws_sensors[2]=0;

  fd(4);
  fd(5);

#define catch_ball { if( walk_shaftenc_dist(50,30,30,2000L,-1) )   \
                     {                                             \
                         walk_shaftenc_dist(-100,-85,20,2000L,-1); \
                     }                                             \
                   }

  if( pos==IPOS_STR )
  {
      walk_shaftenc_dist(100,85,75,0L,-1);
      catch_ball;
  }
  if( pos==IPOS_BACK )
  {
      walk_shaftenc_dist(-100,-65,65,0L,-1);
      diff_turn(-75,-60,45,0L);
      catch_ball;
  }
  if( pos==IPOS_WALL )
  {
      walk_shaftenc_dist(-50,-30,6,1500L,-1);
      diff_turn(75,60,24,0L);
      walk_shaftenc_dist(100,65,3*25,0L,-1);
      catch_ball;
  }
  if( pos==IPOS_CENTER )
  {
      walk_shaftenc_dist(-50,-30,6,1500L,-1);
      diff_turn(-75,-60,23,0L);
      walk_shaftenc_dist(100,65,3*25,0L,-1);
      catch_ball;
  }
  if( majority==-1 )
      majority=1-read_light_sensor(LS_CENTER);
  ws_sensors[0]=LS_CENTER;
  ws_sensors[1]=0;

  /* looping until detecting line */
  while( walk_shaftenc_dist(50,30,0,3000L,majority) )
      walk_shaftenc_dist(-100,-85,30,2000L,-1);

  printf("Turning.\n");
  diff_turn(100,85,27,0L);
  if( timeout_walk_till_hit(-100,SP_ACTUAL_REAR_RIGHT,SP_ACTUAL_REAR_LEFT,1000L,3)!=-1 )
  {
      walk_shaftenc_dist(100,85,40,3000L,-1);
      diff_turn(100,85,54,0L);
      walk_shaftenc_dist(100,85,10,3000L,-1);
      if( timeout_walk_till_hit(100,SP_ACTUAL_FRONT_RIGHT,SP_ACTUAL_FRONT_LEFT,1000L,3)!=-1 )
      {
          walk_shaftenc_dist(-100,-85,15,3000L,-1);
          diff_turn(-100,-85,25,0L);
          wall_following(100,200,LEFT,8000L);
          ao();
          printf("BACKUP PLAN COMPLETED.\n");
          return;
      }
      walk_shaftenc_dist(-100,-85,10,3000L,-1);
      diff_turn(-100,-85,54,0L);
  }

  walk_shaftenc_dist(100,85,65,4000L,-1);
  ir=get_ir();
  if( ir==IR_FRONT )
  {
      timeout_walk_till_hit(-100,SP_ACTUAL_REAR_RIGHT,SP_ACTUAL_REAR_LEFT,3000L,1);
      walk_shaftenc_dist(100,85,10,500L,-1);
      diff_turn(100,85,42,0L);
      wall_following(100,200,LEFT,8000L);
      ao();
      printf("ESCAPED OPPONENT ON LINE (MIDDLE).\n");
      return;
  }
  walk_shaftenc_dist(100,85,60,4000L,-1);
  ir=get_ir();
  if( ir==IR_FRONT )
  {
      timeout_walk_till_hit(-100,SP_ACTUAL_REAR_RIGHT,SP_ACTUAL_REAR_LEFT,4000L,1);
      walk_shaftenc_dist(100,85,10,500L,-1);
      diff_turn(100,85,42,0L);
      wall_following(100,200,LEFT,8000L);
      ao();
      printf("ESCAPED OPPONENT ON LINE (END).\n");
      return;
  }
  timeout_walk_till_hit(100,SP_ACTUAL_FRONT_RIGHT,SP_ACTUAL_FRONT_LEFT,1000L,3);
  
  ir=get_ir();
  printf("IR=%d\n",ir);
  if( ir==IR_LEFT || ir==IR_FRONT)
  {
      timeout_walk_till_hit(-100,SP_ACTUAL_REAR_RIGHT,SP_ACTUAL_REAR_LEFT,4000L,1);
      walk_shaftenc_dist(100,85,10,500L,-1);
      diff_turn(100,85,42,0L);
      wall_following(100,200,LEFT,8000L);
      ao();
      printf("ESCAPED OPPONENT W/ 2ND BALL.\n");
      return;

  } else
  {
      walk_shaftenc_dist(-100,-85,10,500L,-1);
      diff_turn(100,60,23,0L);
      wall_following(100,200,RIGHT,1000L);
      ir=get_ir();
      printf("IR=%d\n",ir);
      if( ir==IR_FRONT )
      {
          diff_turn(100,60,23,0L);
          timeout_walk_till_hit(-100,SP_ACTUAL_REAR_RIGHT,SP_ACTUAL_REAR_LEFT,1500L,2);
          walk_shaftenc_dist(100,85,125,4000L,-1);
          timeout_walk_till_hit(-100,SP_ACTUAL_REAR_RIGHT,SP_ACTUAL_REAR_LEFT,4000L,1);
          walk_shaftenc_dist(-100,-85,10,500L,-1);
          diff_turn(-100,-60,28,0L);
          wall_following(100,200,LEFT,7000L);
      } else
      {
          wall_following(100,200,RIGHT,7000L);
      }
  } 
  ao();
  printf("ORIENT DONE.\n");
}
void escape()
{
    int count[2];
    int i;
    printf("escaping from their side.\n");

    for( ;; )
    {
        for( i=0;i<2; i++ )
            count[i]=0;
        for( i=0;i<NSENSORS;i++ )
            count[read_light_sensor(i)]++;
        if( count[majority]>=2 )
            break;
        i=timeout_walk_till_hit(100,SP_ACTUAL_FRONT_RIGHT,SP_ACTUAL_FRONT_LEFT,4000L,1);
        if( i!=-1 )
            i=timeout_walk_till_hit(100,SP_ACTUAL_FRONT_RIGHT,SP_ACTUAL_FRONT_LEFT,1000L,2);
        while(i!=-1 )
        {
            walk_shaftenc_dist(-100,-85,10,1000L,-1);
            diff_turn(-100,-85,30,3000L);
            wall_following(100,200,RIGHT,4000L);
            i=timeout_walk_till_hit(100,SP_ACTUAL_FRONT_RIGHT,SP_ACTUAL_FRONT_LEFT,1000L,2);
        }
        walk_shaftenc_dist(-100,-85,10,1000L,-1);
        diff_turn(100,85,30,0L);
    }
}

void cleanup()
{
    int count[2],front[2],back[2],vals[NSENSORS];
    int i;
    for( i=0;i<2;i++ )
    {
        count[i]=front[i]=back[i]=0;
    }
    printf("CLEANUP...\n");
    for( i=0;i<NSENSORS; i++ )
    {
        vals[i]=read_light_sensor(i);
        if( i==LS_ACTUAL_FRONT_LEFT || i==LS_ACTUAL_FRONT_RIGHT )
            front[vals[i]]++;
        else back[vals[i]]++;
        count[vals[i]]++;
    }
    if( count[majority]==NSENSORS )
    {
        return;
    }
    if( count[1-majority]==NSENSORS )
    {
        escape();
        return;
    }
    if( front[majority]!=0 && back[majority]==0 )
    {
        walk_shaftenc_dist(100,85,0,2000L,-1);
        cleanup();
    }
    if( front[majority]==0 && back[majority]!=0 )
    {
        walk_shaftenc_dist(-100,-85,0,2000L,-1);
        cleanup();
    }
    if( vals[LS_ACTUAL_FRONT_LEFT]==majority )
    {
        diff_turn(100,85,20,0L);
        walk_shaftenc_dist(100,85,0,2000L,-1);
        cleanup();
    } else
    {
        diff_turn(-100,-85,20,0L);
        walk_shaftenc_dist(100,85,0,2000L,-1);
        cleanup();
    }
}

void main()
{
    calibrate();
    if( !MANUAL_ORIENT )
        start_machine(LSP_CENTER);
    else
      ir_transmit_on();
    orient();
    cleanup();
    disable_servos();
    if( MANUAL_ORIENT )
      ir_transmit_off();
}