/* place your #defines and global variable declarations here */
#define clawmotor 1
#define clawdigitalsensor analog(13)
#define clawanalogsensor analog(3)
#define barrelsensor digital(7)
#define armmotor 3
#define armsensor 0
#define frontsensor digital(7)
#define backsensor digital(9)
#define sidesens1 digital(10)
#define sidesens2 digital(11)
#define sidesens3 digital(14)
#define sidesens4 digital(15)
#define motor1 0
#define motor2 2


void program(){
  int p; 
  fwd();
  turn();
  fwd();
  turn();
  barrel(); 
  height(2);
  p = pickup();
  if(p==0){/*run one more time.in final program but...*/
    alloff(); 
    printf("Press start to continue");
    while(start_button()==0){}}
  /*fwd2*/
  turn2();/**!!**/
  backward();
  /*turn1*/
  /*backward()*/
}


/* put your functions and subroutines here */

int pickup(){
  int c; 
  c = close1();
    if(c==0){
      return 1;}
    else{return 0;}}

int close1(){
  motor(clawmotor,40);
  while(1){
    if(clawdigitalsensor==1 && barrelsensor==1){
      motor(clawmotor,0);
      return 0;}
    if(clawanalogsensor<50){
      motor(clawmotor,0);
      return 1;}}}

void open(){
  motor(clawmotor,-40);
  if(clawanalogsensor>230/**/){
    motor(clawmotor,0);}}

void barrel(){
 height(4);
 open();
 motor(motor1, 40);
 motor(motor2, 40);
 while(barrelsensor==0){}
 alloff();}

void height(int h){
  int a, target; /* 66(up)-107(down) +=up */
  if (h<0){
    target=105;}
  else{
    if (h>10){
      target=67;}
    else{
      if (h>=0||h<=10){
	target=105-38*h/10;}}
    while (a<target){
      a=analog(armsensor);
      motor(armmotor,-40);}
    while (a>target){
      a=analog(armsensor);
      motor(armmotor,40);}
    motor(armmotor,0);}}

int twitch(){
  motor(motor2, -80);
  motor(motor1, -80);
  sleep(2.0);/**!!**/
  motor(motor1, 60);
  motor(motor2, 80);
  while(1){
    if(frontsensor==1){
      alloff();
      return 1;}
    else{
      if(sidesens3==1 || sidesens4==1){
        alloff();
	return 2;}
    }}}

void turn2(){
 while(sidesens1==0 && sidesens2==0){
   motor(motor1, -80);}}

void turn(){
  int t;
  t=1;
 while(t==1){
   t = twitch();}}

int fwd(){
 while(1){
    if(sidesens3==1 && sidesens4==1){}
       motor(motor1, 80);
       motor(motor2, 80);
    if (sidesens3==1 && sidesens4==0){
      motor(motor2, 80);
      motor(motor1, 0);}      
    if (sidesens3==0 && sidesens4==1){
      motor(motor1, 80);
      motor(motor2, 0);}
    if (sidesens3==0 && sidesens4==0){
      motor(motor1, 60);
      motor(motor2, 40);/*!!*/ }
    if(frontsensor==1){
      alloff();
      return 1;}}}

int backward(){
  while(1){
    if(sidesens1==1 && sidesens2==1){}
       motor(motor1, 80);
       motor(motor2, 80);
    if (sidesens1==1 && sidesens2==0){
      motor(motor1, 80);
      motor(motor2, 0);}      
    if (sidesens1==0 && sidesens2==1){
      motor(motor2, 80);
      motor(motor1, 0);}
    if (sidesens1==0 && sidesens2==0){
      motor(motor2, 80);
      motor(motor1, 60);/*!!*/ }
    if(backsensor==1){
      alloff();
      return 1;}}}


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