/* place your #defines and global variable declarations here */
#define motor_claw 2
#define motor_up 1
#define motor_go 0
#define motor_turn 3
/*#define digital_claw 1*/
#define digital_button 7
#define digital_grab 9
#define digital_bump 11
#define digital_side 5
#define analog_claw 0
#define analog_foam 2
#define analog_wheels 4
#define claw_max 104
#define claw_min 74
#define wheels_range 60
int wheels_eq;

void program()
{
wheels_eq = analog(analog_wheels);
search_go();
hit_placebo();
take_foam();
retur();
open_claw();
}

/* MAIN PROCEDURES */

/* There and back again */

void search_go()
{
int a;
motor(motor_go,60);
while (digital(digital_bump)==0)
  {
    search_light();
  }
motor(motor_go,0);
}


void retur()
{
motor(motor_go,-40);
sleep(2.0);
motor(motor_go,10);
turn_around();
if (analog(analog_foam) < 50)
turn_around();
search_go();
}


/* Take the chemical! */

void hit_placebo()
{
motor(motor_go,-10);
sleep(0.5);
turn_wheels(80);
sleep(2.0);
while(digital(digital_side)==0)
motor(motor_go,10);
turn_wheels(-80);
sleep(0.5);
motor(motor_go,0);
}

void take_foam()
{
claw_position(60);
turn_wheels(80);
sleep(2.0);
motor(motor_go,10);
sleep(0.5);
motor(motor_go,0);
pick_up();
if(digital(digital_button)==0)
  take_foam();
claw_position(40);
}

/* SUBPROCEDURES */

/* There and back again */

void search_light()
{
int a;
turn_wheels(15);
if (slope(analog_foam) < 0)
  {turn_wheels(-15);
  sleep(0.3);}
sleep(0.1);
while ((slope(analog_foam) > 0) && (digital(digital_bump) == 0))
     {};
turn_wheels(0);
}



int slope(int c)
{
int a;
int b;
a = analog(c);
sleep(0.3);
b = analog(c);
b = b - a;
return b;
}


void turn_around()
{
turn_wheels(90);
sleep(2.0);
while(slope(analog_foam)<0)
  {};
sleep(1.0);
search_light();
}



void turn_wheels(int p)
{
int n;
n = wheels_eq + (p * wheels_range) / 100;
if (analog(analog_wheels) > n)
  motor(motor_turn,100);
else
  {
    if (analog(analog_wheels) < n)
      motor(motor_turn,-100);
  }
while((analog(analog_wheels) != n) && (digital(digital_bump) == 0))
  {}
motor(motor_turn,0);
}



/* Take the chemical! */

void pick_up()
{
open_claw();
claw_position(10);
close_claw();
claw_position(60);
}

void claw_position(int p)
{
int n;
n = claw_min + ( (100-p) * (claw_max - claw_min)/100);
if (claw_min <= n & n <= claw_max)
{
if (analog(analog_claw) > n)
  motor(motor_up,-80);
else 
  {
  if (analog(analog_claw) < n)
  motor(motor_up, 80);
 }
while(analog(analog_claw) != n)
  {}
motor(motor_up,0);
  }
else printf("\n Check your number ha ha ha");
}


void open_claw()
{
close_claw();
motor(motor_claw,80);
sleep(4.0);
motor(motor_claw,0);
}

void close_claw()
{
if ((digital(digital_button)==0) || (digital(digital_grab)==0))
  motor(motor_claw,-80);
while ((digital(digital_button)==0) || (digital(digital_grab)==0))
  {};
motor(motor_claw,0);
}


/* UNIMPORTANT STUFF THAT I'M TOO LAZY TO DELETE */


void claw_up()
{
if (analog(analog_claw)>claw_min)
  motor(motor_up,80);
while(analog(analog_claw)>claw_min)
  {};
motor(motor_up,0);
}

void claw_down()
{
if (analog(analog_claw)<claw_max)
  motor(motor_up,-80);
while(analog(analog_claw)<claw_max)
  {};
motor(motor_up,0);
}

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


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

}

char menu_choices[7][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","Any button =    Show Analogs","Any button =    Show Digitals"};

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, 7);

    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);
      }
      if (menu_choice == 9) {
	while (start_button() || stop_button()) {}
	display_analogs();
      }
      if (menu_choice == 10) {
	while (start_button() || stop_button()) {}
	display_analogs();
      }
      if (menu_choice == 11) {
	while (start_button() || stop_button()) {}
	display_digitals();
      }
      if (menu_choice == 12) {
	while (start_button() || stop_button()) {}
	display_digitals();
      }

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

    }
    
  }

}

void display_digitals()
{

  int i, a;

  while (!start_button() && !stop_button()) {

    printf("digitals 7->15: ");

    for(i=7; i<=15; i++) {
      a = digital(i);
      printf("%d",a);    
    }
    printf("\n");

    sleep(0.1);
    
  }

}

void display_analogs()
{

  int i, a;

  while (!start_button() && !stop_button()) {

    for(i=0; i<=6; i++) {
      a = analog(i);
      if (a < 10) { printf(" "); }
      if (a < 100) { printf(" "); }
      printf("%d ",a);    
    }
    printf("\n");

    sleep(0.1);
    
  }

}
  
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;
  }
}