/* place your #defines and global variable declarations here */
#define motorx 1
#define motory 3
#define motorxsens analog(0)
#define motorysens analog(2)
#define minspeedx 20
#define xmax 215
#define ymax 195
#define shellx 20
#define oscilx 50
#define minspeedy 15
#define shelly 20
#define oscily 30 

void program(){
  printf("Press Start to begin!\n");
  while(start_button()==0){}
  rttriangle();
  printf("Press Start to continue\n");
  while(start_button()==0){}
  square();
  printf("Press Start to continue\n");
  while(start_button()==0){}
  spiral(90,30);
  printf("Press Start to continue\n");
  while(start_button()==0){}
  triangle();
  printf("Owari!\n");
}

/* put your functions and subroutines here */
int readx(){
  /*domain: (0,215)*/
  int a;
  a=231-motorxsens;
  return a;
}

int ready(){
  /* range:(0,195)*/
  int b;
  b=motorysens-42;
  return b;
}

int coastx(int x, int s, int p, int a){
  int d;
  int z;
  if(x<0){x=0;}
  if(x>215){x=215;} 
  d=x-readx(); 
  while(((d<=-s) || (d>=s)) && (safelimx()==1)){
    d=x-readx();
    motor(motorx,d);}
  while(((d<=-p) || (d>=p)) && (safelimx()==1)){
    d=x-readx();    
    if(d<0){
      motor(motorx,-a);}
    if(d>0){  
      motor(motorx,a);}}
  motor(motorx, 0);
  sleep(1.0);
  z=x-readx();
  return z;
}

int test(int x, int s, int p, int a){
  int c=0;
  int d;
  int z;
  int dold;
  if(x<0){x=0;}
  if(x>215){x=215;} 
  d=x-readx(); 
  while(((d<=-s) || (d>=s)) && (safelimx()==1)){
    d=x-readx();
    motor(motorx,d);}
  while(((d<=-p) || (d>=p)) && (safelimx()==1) && (c<=100)){
    dold=d;
    d=x-readx();
    if(d<0){
      motor(motorx,-a);}
    if(d>0){  
      motor(motorx,a);}
    if((d-dold)==0){
      c=c+1;}
    printf("d is %d",d-dold);}
  motor(motorx, 0);
  sleep(1.0);
  z=x-readx();
  return z;
}

int gotox(int x){
  int c=0;
  int d;
  int z;
  int dold;
  if(x<0){x=0;}
  if(x>xmax){x=xmax;} 
  d=x-readx(); 
  while(((d<=-shellx) || (d>=shellx)) && (safelimx()==1)){
    d=x-readx();
    motor(motorx,d);}
  while((safelimx()==1) && (c<=oscilx)){
    dold=d;
    d=x-readx();
    if(d<0){
      motor(motorx,-(minspeedx));}
    if(d>0){  
      motor(motorx,(minspeedx));}
    if((d-dold)==0){
      c=c+1;}}
  motor(motorx, 0);
  sleep(1.0);
  z=x-readx();
  return z;
}

int gotoy(int y){
  int c=0;
  int d;
  int z;
  int dold;
  if(y<0){y=0;}
  if(y>ymax){y=ymax;} 
  d=y-ready(); 
  while(((d<=-shelly) || (d>=shelly)) && (safelimy()==1)){
    d=y-ready();
    motor(motory,d);}
  while((safelimy()==1) && (c<=oscily)){
    dold=d;
    d=y-ready();
    if(d<0){
      motor(motory,-(minspeedy));}
    if(d>0){  
      motor(motory,(minspeedy));}
    if((d-dold)==0){
      c=c+1;}}
  motor(motory, 0);
  sleep(1.0);
  z=y-ready();
  return z;
}

int line(int x2, int y2, int stepsize){
  int dx;
  int dy;
  int c1=0;
  int c2=0;
  int x1=readx();
  int y1=ready();
  dx=x2-x1;
  dy=y2-y1;
  while((c1<stepsize) && (c2<stepsize)){
    gotox((x1+((c1*dx)/stepsize)));
    c1=c1+1;
    gotoy((y1+((c2*dy)/stepsize)));
    c2=c2+1;}
  return 1;
}

int safelimx(){
  if((readx()<=0) || (readx()>=xmax)){
    motor(motorx, 0);
    return 0;}
  return 1;}

int safelimy(){
  if((ready()<=0) || (ready()>=ymax)){
    motor(motory, 0);
    return 0;}
  return 1;}
    
int square(){
  gotox(60);
  gotoy(70);
  gotox(130);
  gotoy(160);
  return 1;}

int triangle(){
  gotox(120);
  line(70,120,50);
  line(30,30,50);
  return 1;
}

int rttriangle(){
  gotox(120);
  line(30,130,50);
  gotoy(30);
  return 1;
}

int spiral(int a, int b){
  gotox(((readx())+a));
  while((a>=10) && (b>=10)){
    a=((14*a)/16);
    b=((14*b)/16);
    gotoy(((readx())+b));
    gotox(((ready())-a));
    a=((14*a)/16);
    b=((14*b)/16);
    gotoy(((readx())-b));
    gotox(((ready())+a));}
  return 1;
}

/*int stoppt( int x, int y){
  while (1){
    if(readpt==*/
  
/*********************************************************************/
/********* 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;
  }
}