/**************************************************************/
/* template.c: provides a menued interface to the Handyboard, */
/*             allowing you to test motors, sensors, etc...   */
/*	       while your program is loaded                   */
/*                                                            */
/* Usage: Turn the User Knob to select a menu option.         */
/*        Use the Start and Stop buttons to make selections   */
/*                                                            */
/* Ben "Max" Davis, 2/99, for MIT ESG Robotics Seminar SP.285 */
/*                  some code used from menu.c by Anne Wright */
/*                                                            */
/* v1.1, 2/00, minor modifications, motor power is now 100%   */
/*                                                            */
/**************************************************************/



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

void program()
{

  /* declare your variables here */

  /* your program goes here */

}

/* put your functions and subroutines here */




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


void main()
{

  printf("template.c v1.1 updated 2/1/00\n");
  sleep(1.5);

  while (1==1) {
    ao();
    menu();  
    ao();
    program();
    ao();
    sleep(1.0);
  }

}

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 = 100;					      

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