#define motor_claw 2
#define motor_up 1
#define motor_go 3
#define motor_turn 0
#define digital_button 7
#define digital_grab 9
#define digital_bump 11
#define digital_side 13
#define analog_claw 0
#define analog_foam 2
#define analog_wheels 4
#define claw_max 104
#define claw_min 74
#define wheels_range 25
/* #define wheels_eq */

int check;
int data;
int wheels_eq;

void program()
{


int speed;
int turn;
int data2;
data=0;
check = 0;
sony_init(1); 
wheels_eq = analog(analog_wheels); 

while (check==0)
  {
    /*    printf("checking data\n"); */
    data = ir_data(1);
        if (data != 0)
      {
  	printf("data = %d\n", data);
        decide(data);
      }
  }
}



void decide(int data)
{
  int speed;
  int data2;
   if (data == cd0)
    check=1; 
  if (data == cdplay)
    {
      printf("going forward\n");
      sleep(2.);
      data2=ir_data(1);
      if (data2 == cd1)
	speed = 10;
      if (data2 == cd2)
	speed = 20;
      if (data2 == cd3)
	speed = 30;
      if (data2 == cd4)
	speed = 40;
      if (data2 == cd5)
	speed = 50;
      if (data2 == cd6)
	speed = 60;
      if (data2 == cd7)
	speed = 70;
      if (data2 == cd8)
	speed = 80;
      if (data2 == cd9)
	speed = 90;
      printf("with speed %d\n", speed);
      motor(motor_go,speed);
    } 

  if (data == cd2)
    {
      printf("going backward\n");
      sleep(2.);
      data2 = ir_data(1);
      if (data2 == cd1)
	speed = -10;
      if (data2 == cd2)
	speed = -20;
      if (data2 == cd3)
	speed = -30;
      if (data2 == cd4)
	speed = -40;
      if (data2 == cd5)
	speed = -50;
      if (data2 == cd6)
	speed = -60;
      if (data2 == cd7)
	speed = -70;
      if (data2 == cd8)
	speed = -80;
      if (data2 == cd9)
	speed = -90;
      printf("with speed %d\n", speed);
      motor(motor_go,speed);
    }
  
  if (data == cdrew)
    {
      printf("turning left\n");
      motor(motor_turn,80);
      while ((ir_data(1) != cdminus) && (analog(analog_wheels) > (wheels_eq - wheels_range)))
	{};
      motor(motor_turn,0);
      
    }
  
  if (data == cdforw)
    {
      printf("turning right\n");
      motor(motor_turn,-90);
      while ((ir_data(1) != cdminus) && (analog(analog_wheels) < (wheels_eq + wheels_range)))
	{};
      motor(motor_turn,0);
    }
  
  if (data == cdpause)
    {
      printf("going straight\n");
      turn_wheels(0);
    }
  
  if (data == cdstop)
    {
      printf("stopping\n");
      motor(motor_go, 0);
    }
  if (data == cd1)
    {
      pick_up();
    }
  if (data == cd4)
    {
      sleep(1.);
      data2=ir_data(1);
       if (data2 == cd1)
	speed = 10;
      if (data2 == cd2)
	speed = 20;
      if (data2 == cd3)
	speed = 30;
      if (data2 == cd4)
	speed = 40;
      if (data2 == cd5)
	speed = 50;
      if (data2 == cd6)
	speed = 60;
      if (data2 == cd7)
	speed = 70;
      if (data2 == cd8)
	speed = 80;
      if (data2 == cd9)
	speed = 90;
      claw_position(speed);
    }
  if (data == cd5)
    open_claw();
  if (data == cd6)
    grab();
  if (data == cd8)
    {
      while (ir_data(1) == 0)
	motor(motor_claw,80);
      motor(motor_claw,0);
    }
  if (data == cd9)
    {
      while (ir_data(1) == 0)
	motor(motor_claw,-80);
      motor(motor_claw,0);
    }
}


void turn_wheels(int p)
{
int n;
n = wheels_eq + (p * wheels_range) / 100;
if (analog(analog_wheels) > n)
  motor(motor_turn,90);
else
  {
    if (analog(analog_wheels) < n)
      motor(motor_turn,-90);
  }
while((analog(analog_wheels) != n) && (digital(digital_bump) == 0))
  {}
motor(motor_turn,0);
}


void pick_up()
{
open_claw();
claw_position(10);
grab();
claw_position(70);
}

void claw_position(int p)
{
int n;
n = claw_min + ( (100-p) * (claw_max - claw_min)/100);
if (claw_min <= n & n <= claw_max)
{
if (analog(analog_claw) > n)
  motor(motor_up,-80);
else 
  {
  if (analog(analog_claw) < n)
  motor(motor_up, 80);
 }
while(analog(analog_claw) != n)
  {}
motor(motor_up,0);
  }
else printf("\n Check your number ha ha ha");
}


void open_claw()
{
close_claw();
motor(motor_claw,80);
sleep(5.0);
motor(motor_claw,0);
}

void close_claw()
{
if (digital(digital_button)==0) 
  motor(motor_claw,-80);
while ((digital(digital_button)==0) && (digital(digital_grab)==0))
  {};
motor(motor_claw,0);
}

void grab()
{
if ((digital(digital_button)==0) && (digital(digital_grab)==0))
  motor(motor_claw,-80);
while ((digital(digital_button)==0) && (digital(digital_grab)==0))
  {};
sleep(2.0);
motor(motor_claw,0);
}































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