#define irleft 15     /*the port that the ir sensor plugs into*/
#define irfront 14
#define irright 13
#define irback 12
#define freq 100      /*receives an ir frequency either 100hz or 125hz */
#define rmotor 0      /* port for right motor*/
#define lmotor 1      /* port for left motor*/
#define leftscale 50   /* a factor to adjust the */
#define rightscale 100

void program()
{
  ir_receive_on();
  set_ir_receive_frequency(freq);
  while(!stop_button()){
    sleep(0.1);
    steer();
  }
}
  


/*  printf("digital1 is %d digital2 is %d \n",ir_counts(13),ir_counts(15)) */


/* put your functions and subroutines here */



void steer()
{
  int lmotorpower;
  int rmotorpower;
  int f;
  int r;
  int b;
  int l;
  int x;
  int y;
  int ircounts_f=ir_counts(irfront);
  int ircounts_r=ir_counts(irright);
  int ircounts_l=ir_counts(irleft);
  f=0;
  r=0;
  b=0;
  l=0;
  
  if (ircounts_l>=3)
    {
      l=1;
    }
  if( ircounts_r >= 3 )
    {
      r=1;
    }
  if( ircounts_f >= 3 )
    {
      f=1;
    }
  if( (l+r+f)==0 )
    {
      b=1;
    }
  
  x = r-l;
  y = f-b;
  
  lmotorpower = leftscale*(y+x);
  rmotorpower = rightscale*(y-x);
  
  motor(lmotor, lmotorpower);
  motor(rmotor, rmotorpower);
  
}






void transmit()
{
  ir_transmit_on();
  set_ir_transmit_frequency(100);
  while(!stop_button())
    {
    }
  ir_transmit_off();
}
  
float abs(float x)
{
  if(x<0.0)
    {
      x=-x;
    }
  return x;
}


/*********************************************************************/
/********* FROM HERE ON ARE MENU ROUTINES: LEAVE AS IS ***************/
/*********************************************************************/


void main()
{
  while (1==1) {
    ao();
    menu();  
    ao();
    program();
  }
  
}

char menu_choices[5][32] = {"Any button = run user program","MOTOR: Start=FWD  0  : Stop=RVS","MOTOR: Start=FWD  1  : Stop=RVS","MOTOR: Start=FWD  2  : Stop=RVS","MOTOR: Start=FWD  3  : Stop=RVS"};

int menu_motorstate = 0;					      
int menu_choice = 9;						      
int menu_motorpower = 80;					      

void menu() {
  
  while (1) {
    
    while (stop_button() || start_button()) {}
    sleep(0.05);
    while (stop_button() || start_button()) {}
    sleep(0.05);
    while (stop_button() || start_button()) {}
    sleep(0.05);
    while (stop_button() || start_button()) {}
    
    menu_choice = select_string(menu_choices, 5);

    if ((menu_choice == -1) || (menu_choice == 0)) {
      printf("\n\n");
      return;
    }
    else {
      if (menu_choice == 1) {
	motor(0, menu_motorpower);
      }
      if (menu_choice == 2) {
	motor(0, -menu_motorpower);
      }
      if (menu_choice == 3) {
	motor(1, menu_motorpower);
      }
      if (menu_choice == 4) {
	motor(1, -menu_motorpower);
      }
      if (menu_choice == 5) {
	motor(2, menu_motorpower);
      }
      if (menu_choice == 6) {
	motor(2, -menu_motorpower);
      }
      if (menu_choice == 7) {
	motor(3, menu_motorpower);
      }
      if (menu_choice == 8) {
	motor(3, -menu_motorpower);
      }

      while (stop_button() || start_button()) {}
      
      ao();

    }
    
  }

}

int select_string(char choices[][],int n)
{

  int selection,last_selection=-1,coarseness;
  if(n>_array_size(choices))
    n=_array_size(choices);

  coarseness=255/(n-1);

  while(!(stop_button() || start_button()))
    {
      selection=(knob())/coarseness;
      if(selection!=last_selection)
	{
	  printf("%s\n",choices[selection]);
	  sleep(0.1);
	  last_selection=selection;
	}
    }
  if (start_button()) {
    return selection * 2 - 1;
  }
  else {
    return selection * 2;
  }
}