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

#define hand 1
#define arm 3
#define opensensor 1
#define upsensor 1
int hdirect;
int adirect;

void program() {
  hdirect = direct(hand,hand) * opensensor;
  adirect = direct(arm,arm) * upsensor;
}

int direct(int motport,int sensport) {
  int timer = 1;
  while (1) {
    int diff = change(timer,1,motport,sensport);
    if ( abs(diff) > 6 ) {
      return diff / abs(diff);}
    else {
      diff = change(timer,-1,motport,sensport);
      if ( abs(diff) > 6) {
	return -1 * diff / abs(diff);}}
    timer++;}
}

int change(int time,int direc,int motport,int sensport) {
  int pos1;
  int pos2;
  pos1 = analog(sensport);
  motor(motport,50 * direc);
  sleep(.5 * (float) time);
  motor(motport,0);
  pos2 = analog(sensport);
  return pos2 - pos1;
}

int close(int motport,int sensport) {
  int diffmax = 0;
  int diff = 3;
  while (analog(hand) > 5) {
    if (abs(diff) > diffmax - 3) {
    diff = change(5,-1*hdirect,motport,sensport);
    if (abs(diff) > diffmax) {
      diffmax = abs(diff);}}
    else {return 1;}}
  return 0;
}

void puthand(int position) {
  int diff = -1;
  while(diff != 0) {
    diff = position - analog(hand) / 25;
    motor(hand,hdirect * diff / abs(diff) *100);}
motor(hand,0);
}

void putarm(int position) {
  int diff = -1;
  while(diff != 0) {
    diff = position - analog(arm) / 25;
    motor(arm,adirect * diff / abs(diff) *100);}
motor(arm,0);
}

int abs(int number) {
  if (number > 0) {return number;}
  else {return number * -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;
  }
}