/* Program to test MyroC commands for retrieving sensor data
 *
 * This program and all MyroC software is licensed under the Creative Commons
 * Attribution-Noncommercial-Share Alike 3.0 United States License.
 * Details may be found at http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 */

#include <stdio.h>
#include <unistd.h>     // needed for sleep command
#include <stdlib.h>     // needed for exit command
#include "MyroC.h"

int main ()
{
  printf ("test of sensor functions\n");

  //rSetOpeningExchange ('y');
  //rSetBluetoothEcho ('y');

  //rConnect ("/dev/rfcomm0");
  //rConnect ("/dev/tty.Fluke2-0958-Fluke2");
  //rConnect ("/dev/tty.IPRE6-365897-DevB");
  //rConnect ("00:1E:19:01:0E:8C");  // IPRE365827 - post
  //rConnect ("B4:D8:A9:00:09:13"); // robot at workstation wilkes
  rConnect ("B4:D8:A9:00:0D:40"); // loaner-4
  rSetForwardness ("scribbler-forward");


  int sensorData [3];

  rSetBluetoothEcho ('y');

  /* use rBeep to get echo of sensor data */
  rBeep (1.0, 880);

  rSetBluetoothEcho ('n');

  /* scribbler sensors */
  printf ("light sensors\n");
  printf ("\tleft light sensor:    %d\n", rGetLightTxt("left", 1));
  printf ("\tmiddle light sensor:  %d\n", rGetLightTxt("middle", 10));
  printf ("\tright light sensor:   %d\n", rGetLightTxt("right", 10));
  rGetLightsAll (sensorData, 10);
  printf ("\tarray data:  left: %d, middle: %d, right: %d\n",
          sensorData[0], sensorData[1], sensorData[2]);

  printf ("\nir sensors\n");
  printf ("\tleft ir sensor:    %d (1=obstacle)\n", rGetIRTxt("left", 10));
  printf ("\tright ir sensor:   %d (0=no obstacle)\n", rGetIRTxt("right", 10));
  rGetIRAll (sensorData, 10);
  printf ("\tarray data:  left: %d, right: %d\n",
              sensorData[0], sensorData[1]);
  printf ("\nline sensors\n");

  printf ("    Scribbler forward\n");
  rGetLine (sensorData, 10);
  printf ("\tarray data:  left: %d, right: %d\n",
          sensorData[0], sensorData[1]);
  printf ("    FlukeScribbler forward\n");
  rGetLine (sensorData, 10);
  printf ("\tarray data:  left: %d, right: %d\n",
          sensorData[0], sensorData[1]);

  /* fluke sensors */
  printf ("\nobstacle sensors\n");
  int power;
  printf ("      power   left   middle  right     l/m/r array\n");
  for (power = 100; power <= 200; power+=10)
    {
      printf ("%10d", power);
      printf ("%8d", rGetObstacleTxt("left", 5));
      printf ("%8d", rGetObstacleTxt("middle", 5));
      printf ("%8d", rGetObstacleTxt("right", 5));
      rGetObstacleAll (sensorData, 5);
      printf ("%8d %5d %5d\n", sensorData[0], sensorData[1], sensorData[2]);
   }

  printf ("\ndisplaying camera image, as basis for bright sensors\n");
  Picture pic = rTakePicture();
  rDisplayPicture (&pic, 0.0, "camera image");

  printf ("\nbright sensors\n");
  printf ("\tleft average %d\n",    rGetBrightTxt("left", 10));
  printf ("\tmiddle average %d\n",  rGetBrightTxt("middle", 10));
  printf ("\tright average %d\n",   rGetBrightTxt("right", 10));

  rGetBrightAll (sensorData, 10);
  printf ("\tarray data:  left: %d, middle: %d, right: %d\n",
          sensorData[0], sensorData[1], sensorData[2]);

  rSetBluetoothEcho ('y');
  printf ("battery sensor\n");
  printf ("\treported value:  %lf\n", rGetBattery ());

  /* wrap up with rGetName, to be sure communication in sync */
  printf ("finishing up with robot %s\n", rGetName());

  sleep (10);
  rDisconnect ();
  printf ("test completed\n");
  return 0;

}