#use "expservo.icb"
int results[7];

void 
main ()
{
  int i;
  int pos = 0;
  while (1)
    {
      start_press ();
      init_expbd_servos (1);
      sonar_init ();

      for (i = 0; i < 7; i++)
	{
	  servo1 = i * 666;
	  sleep (0.3);
	  results[i] = filtered_sonar (8) / 100;
	  printf ("doing %d, got %d\n", i, results[i]);
	  sleep (0.3);
	}
      printf ("%d %d %d %d %d %d %d\n", results[0], results[1], results[2], results[3], results[4], results[5], results[6]);
      init_expbd_servos (0);
    }
  while (0)
    {
      int result;
      pos = pos + 20;
      servo0 = pos;
      servo1 = pos;
      printf ("%d %d\n", servo0, servo1);
      //result= sonar_sample();
      //if (result != -1)
      //  printf("%d\n", result);
      //else
      //  printf("@@@@@@@\n");
      //tone((float)(result/8),0.2);
      //sleep(0.5);
    }

}


int 
filtered_sonar (int samples)
{
  int i;
  int sum;
  int temp, readings;

  sum = 0;
  readings = 0;
  for (i = 0; i < samples; i++)
    {
      temp = sonar_sample ();
      if (temp > 0)
	{
	  sum = sum + temp / samples;
	  readings++;
	}
      sleep (0.1);
    }

  if (readings == 0)
    return -1;

  return sum * samples / readings;
}

void 
sonar_init ()
{
  bit_set (0x1026, 0x80);	/* set Digital Input #9 as output */
  bit_clear (0x1021, 1);	/* at TCTL2, */
  bit_set (0x1021, 2);		/* set TIC3 for falling edge */
}



int 
sonar_sample ()
{

  int start_time;
  int dist;

  poke (0x1023, 1);		/* clear tic3 flag */

  start_time = peekword (0x100e);	/* capture start time */

  bit_set (0x1000, 0x80);	/* send init pulse */
  bit_clear (0x1000, 0x80);

  while (peek (0x1000) & 0x1)
    {				/* wait until receive echo */
      hog_processor ();
      if ((peekword (0x100e) - start_time) < 0)
	{
	  /* if too much time has elapsed, abort */
	  return -1;
	}
      defer ();			/* let others run while waiting */
    }

  msleep (10L);			/* give unit time to reset */

  dist = peekword (0x1014) - start_time;
  //    if (dist < 0)
  //      dist = 32767;
  return dist;			/* tic3 has time of echo */
}