int testval;

int test_spin(int dir) {
  drive_left(40*dir);
  drive_right(-1*20*dir);
}

int test_sidedrive(int speed) {
  steer_sideways();
  sleep(.6);
  drive_left(speed);
  drive_right(-speed);
}

int test_sensor(int snum) {
  int lastval;
  if (snum>=16) {analog(snum);} /* read twice for >16 */
  lastval=analog(snum);
  while(!stop_button()) {
    if (analog(snum)!=lastval) {
      printf("Sensor %d value:%d\n",snum, analog(snum));
      lastval = analog(snum);
    }
  }
}

int test_sensors() {
  int kval;
  int lastval;

  while(1) {
    while(!start_button()) {
      kval = (knob() * 33) / 255;
      if (kval != lastval) {
	printf("Test on: %d\n",kval);
	lastval = kval;
      }
    }
    test_sensor(kval);
  }
}

void test_spin_folded(int sangle){
  servo(DRIVE_RIGHT_SERVO,DRIVE_RIGHT_SERVO_CENTER-(20*(sangle/2)));
  servo(DRIVE_LEFT_SERVO,DRIVE_LEFT_SERVO_CENTER+(21*(sangle/2)));
  test_spin(10);
  sleep(.3);
  servo(DRIVE_RIGHT_SERVO,DRIVE_RIGHT_SERVO_CENTER-(20*sangle));
  servo(DRIVE_LEFT_SERVO,DRIVE_LEFT_SERVO_CENTER+(21*sangle));
}
    
void test_orient(int endval, int speed, int sangle) {
  
  int count = 0 ;
  /* int sangle = 40;
     int speed = 40; */

  enable_encoder(7);
  enable_encoder(8);
  reset_encoder(7);
  reset_encoder(8);
  
  servo(DRIVE_RIGHT_SERVO,DRIVE_RIGHT_SERVO_CENTER-(20*(sangle)));
  servo(DRIVE_LEFT_SERVO,DRIVE_LEFT_SERVO_CENTER+(21*(sangle)));
  drive_spin(speed);
  while(read_encoder(7)+read_encoder(8) < endval && count++ < 200) {
    sleep(.05);
  }
  drive_spin(0);
}

void test_hinge(){
  open_hinge();
  sleep(2.0);
  close_hinge();
  sleep(2.0);
}

void test_spin_modulate() {
  int i; 
  start_process(test_bang_detect());
  servo(DRIVE_RIGHT_SERVO,DRIVE_RIGHT_SERVO_CENTER);
  servo(DRIVE_LEFT_SERVO,DRIVE_LEFT_SERVO_CENTER);
  test_spin(10);
  printf("Wheels at:      0 Degrees.\n");
   sleep(3.0);
    for (i=0;i<91;i+=10) {
      servo(DRIVE_RIGHT_SERVO,DRIVE_RIGHT_SERVO_CENTER-(20*i));
      servo(DRIVE_LEFT_SERVO,DRIVE_LEFT_SERVO_CENTER+(21*i)); 
      printf("Wheels at:      %d Degrees.\n",i);
      sleep(5.0);
      if (stop_button())
	break;
      
    }
}

void test_bang_detect() {
  while (!digital(10) && !digital(11)) {}
  ao();
  disable_servos();
}


void init_all() {
  init_drive();
  init_hinge();
}

void test_orient_calibrate() {
  int kval;
  int lastval;

  int speed;
  int angle;
  int endval;

  int pid;

  while(!start_button()) {
    kval = (knob());
    if (kval != lastval) {
      printf("Endval: %d\n",kval);
      lastval = kval;
    }
  }
  endval=kval;
  kval=0;
  while(start_button()){}
  
  while(!start_button()) {
    kval = (knob() * 100) / 255;
    if (kval != lastval) {
      printf("Speed: -%d\n",kval);
      lastval = kval;
    }
  }
  speed=kval;
  kval=0;
  while(start_button()){}

  while(!start_button()) {
    kval = (knob()*90) / 255;
    if (kval != lastval) {
      printf("Angle: %d\n",kval);
      lastval = kval;
    }
  }
  angle=kval;
  kval=0;
  while(start_button()){}
 
  
  while(1) {
    init_hinge();
    close_hinge();
    printf("Ready. S:-%d E:%d A:%d\n",speed,endval,angle);
    start_press();
    pid = start_process(test_bang_detect());
    test_orient(endval,-speed,angle);
    start_press();
    kill_process(pid);
  }
}


/* -----------------------------
  DRIVE TEST Suite:
  On "start", does a series of obscure motions:

  run: drivetest()
  ----------------------------- */
void drivetest() {
  steer_straight();
  drive_forward(50);
  sleep(2.0);
  ao();
  steer_sideways();
  drive_forward(50);
  sleep(1.0);
  ao();
  steer_straight();
  drive_forward(-50);
  sleep(2.0);
  ao();
  steer_sideways();
  drive_forward(-50);
  sleep(1.0);
  ao();
  steer_straight();
}

void shaft_encoder_test() {
  /* Tests shaft encoders on ports 7 and 8.  7 is left. */

  printf("Shaft encoder test\n");
 

  enable_encoder(7);
  enable_encoder(8);
  
  while(!stop_button()) {
    printf("Left %d Right %d\n", read_encoder(7), read_encoder(8));
    sleep(.5);
  }

  printf("Done.\n");
  reset_encoder(7);
  reset_encoder(8);
}