/* place your #defines and global variable declarations here */

#define height_motor 0
#define height_sensor 0
#define claw_motor 1
#define claw_sensor 7
#define good_speed 80
#define shut 0

#define top 1
#define bot 3
#define sensor 8
#define fast 80
#define slow 40


void program()
{
  
  int x=2;
  while 1 {
    while start_button()==0 {}
    
    if (digital(sensor)==x) {
      tone(500,1);
    }
    else{
      x=digital(sensor);
      if x==1 {
	gorightsideup();
      }
      else {
	goupsidedown();
      }
      tone(500,3);
    }
  }
}

void gorightsideup()
{
  motor(top,fast);
  motor(bot,fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,0-fast);
  motor(bot,0-fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,slow);
  motor(bot,fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,0-slow);
  motor(bot,0-fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,fast);
  motor(bot,slow);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,0-fast);
  motor(bot,0-slow);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
}

void goupsidedown()
{

 motor(top,0-fast);
  motor(bot,0-fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,fast);
  motor(bot,fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,0-slow);
  motor(bot,0-fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,slow);
  motor(bot,fast);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,0-fast);
  motor(bot,0-slow);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
  sleep(1.0);
  motor(top,fast);
  motor(bot,slow);
  sleep(3.0);
  motor(top,shut);
  motor(bot,shut);
}


void openclaw() {
  /* declare your variables here */

  /* your program goes here */
  while (digital(claw_sensor)==0) {
    motor (claw_motor, good_speed);
  }
motor(claw_motor, shut);

}

void height(int height_we_want) {
int x=analog(height_sensor);
height_we_want=((height_we_want*35)/10)+45;
if ((height_we_want>80) || (height_we_want<45)) {
beep();
printf("Error: value notbetween 0 - 10\n");
} 
else {
 while  (height_we_want>analog(height_sensor)) {
motor(height_motor,good_speed);
  }
motor(height_motor, shut);
  while (height_we_want<analog(height_sensor)) {
motor (height_motor,0-good_speed);
  }
motor(height_motor, shut);
}
}


/* put your functions and subroutines here */




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