/* procedure to compare rSend methods using read for echo data
   parameters include read 1 character at a time and as many as can be read at once

   rSendA:  original version of rReceive, use read, up to 1 byte at a time
   rSendB:  use read, read as many bytes as available until all obtained

   for accurate timings, mutex semaphores not used in this program
*/

#include <MyroC.h>
#include "../MyroC-utilities.h"
#include <unistd.h>      // for read
#include <sys/socket.h>  // for recv
#include <errno.h>       // for errno
#include <stdio.h>       // for printf
#include <time.h>
#include <stdlib.h>      // for time

int socket_num;

/* variables to count number of loop iterations in each receive version */
int iterA;
int iterB;

/* Sends commands via bluetooth, assuming mutex set*/
/**
 * send bytes to the Scribbler --- same as rSend, EXCEPT
 * @pre                  a mutex has been set to prevent concurrent Bluetooth processiung
 * @param message        the command/data to be sent
 * @param numberOfBytes  how many bytes to transmit
 * @param echo           whether or not (1/0) the Scribbler echoes the command
 * @param sensor_echo    whether or not the Scribbler sends all sensor data
 *                       back after echoing the command
 */
void rSendA (char message[], int numberOfBytes,
                      int echo, int sensor_echo, char * commandText)
{
  /* send message */
  int status = write (socket_num, message, numberOfBytes);

  //printf ("%d bytes written to socket\n", status);

  if (status != numberOfBytes)
    {
      perror ("Error in sending command over Bluetooth\n");
      exit (1);
    }

  /* if Scribbler echoes command get echo, and check echo matches message */
  if (echo)
    {
      int totalEchoed = 0;
      int errorPrinted = 0;
      while (totalEchoed<numberOfBytes)
        { char echoedCommandChar;
          /* read echoed bytes and check they match the message send */
          int readNum = read (socket_num, &echoedCommandChar, 1);
          if (readNum > 0)
            {
              /* bytes should echo correctly, except for bytes 0, 1, and 2
                 of the rMove command */
              if ((!errorPrinted) &&
                  (echoedCommandChar != message[totalEchoed]) &&
                  ((message[0]!= 109) || /* not the rMove command */
                   ((message[0]== 109) && (totalEchoed > 2))))
                { /*
                  printf ("discrepency:  ");
                  */
                  fprintf (stderr, "Bluetooth communication error, ");
                  fprintf (stderr, "starting at byte %d for %s command (code %d)\n",
                           totalEchoed, commandText, message[0]);
                  errorPrinted = 1;
                  /* exit (1); */
                  
                }
              
              /* print 1 byte of data in %u format */
              if (debug)
                {
                  printf ("command echo '%3u'   message '%3u'\n", 
                          echoedCommandChar, message[totalEchoed]);
                }
              totalEchoed += readNum;
            }
        }
    }
  if (sensor_echo)
    { int j = 0;
      while (j < BYTES_OF_SENSOR_DATA)
        { unsigned char echoedSensorChar = 255;
         /* read bytes of echoed sensor data */
          j += read (socket_num, &echoedSensorChar, 1);
          
          if (debug && (echoedSensorChar != 255))
            {
              printf ("sensor data '%3u'\n", echoedSensorChar);
              echoedSensorChar = 255;
            }
        }
    }
}


void rSendB (char message[], int numberOfBytes,
                      int echo, int sensor_echo, char * commandText)
{
  /* send message */
  int status = write (socket_num, message, numberOfBytes);

  //printf ("%d bytes written to socket\n", status);

  if (status != numberOfBytes)
    {
      perror ("Error in sending command over Bluetooth\n");
      exit (1);
    }

  /* if Scribbler echoes command get echo, and check echo matches message */
  if (echo)
    {
      /* read bytes of echoed command */
       int total_read = 0;
       char command_echo [numberOfBytes];

       while (total_read < numberOfBytes)
         {
            total_read += read (socket_num, command_echo+total_read, 
	                        numberOfBytes-total_read);
         }

       /* check echoed bytes match the message sent */
       /* bytes should echo correctly, except for bytes 0, 1, and 2 of the rMove command */
       int i;
       if (message[0] == 109)
	 i = 3;  // start with byte 3 of rMotors command
       else
	 i = 0;  // start with byte 0 for all other commands
       while (i < numberOfBytes)
	 {
	   if (command_echo[i] != message[i])
	     {
                  printf ("discrepency:  ");
                  fprintf (stderr, "Bluetooth communication error, ");
                  fprintf (stderr, "starting at byte %d for %s command (code %d)\n",
                           i, commandText, message[0]);
                  break;
	     }
	   i++;
	 }

       /* print sensor data in debug mode */
       if (debug)
         {
	   int j;
           for (j = 0; j < numberOfBytes; j++)
                  printf ("command echo '%3u'   message '%3u'\n", 
			  command_echo[j], message[j]);
         }
    }

}



/**
 * Receives data via bluetooth
 * @param message       a pointer to allocated space, where the data will be stored
 * @param numberOfEchoBytes  number of bytes to be received from the Scribbler 
 * @pew                 space pointed to by message >= numberOfEchoBytes
 */
void rReceiveA (unsigned char * message, int numberOfBytes)
{ int total_read;
  iterA = 0;
  for (total_read=0; total_read<numberOfBytes; )
    {
      if (read (socket_num, message+total_read, 1) > 0)
        {
          // printf("received '%u'\n", message[total_read]&255);
          total_read++;
        }
      iterA++;
    }  
}

void rReceiveB (unsigned char * message, int numberOfBytes)
{ int total_read = 0;
  iterB = 0;
  while (total_read < numberOfBytes)
    {
      total_read += read (socket_num, message+total_read, numberOfBytes-total_read);
      iterB++;
    }
}

/* test and compare all rReceive options
   @parm message:  the command to be issued
   @parm numberOfBytes:  number of response bytes expected
   @printResult:  0 - do not print returned data; 1 - print returned data
*/
void testMethods (char * message, int message_len,
		  int COMMAND_ECHO, int DATA_ECHO,
		  int numberOfBytes, int printResult)
{
  unsigned char * newNameA = malloc (sizeof(char) * numberOfBytes);
  unsigned char * newNameB = malloc (sizeof(char) * numberOfBytes);

  clock_t start_time, end_time;
  double elapsed_time;

  start_time = clock ();
  rSendA (message, message_len, COMMAND_ECHO, DATA_ECHO, "Command");
  end_time = clock ();
  printf ("%3d", iterA);
  elapsed_time = (end_time - start_time) / (double) CLOCKS_PER_SEC;
  printf ("  %7.5lf  ", elapsed_time);

  //printf ("end version A\n");

  start_time = clock ();
  rReceiveA (newNameA, numberOfBytes);
  end_time = clock ();
  printf ("%3d", iterA);
  elapsed_time = (end_time - start_time) / (double) CLOCKS_PER_SEC;
  printf ("  %7.5lf  ", elapsed_time);

 if (printResult)
    {
      int i;
      for (i = 0; i < numberOfBytes; i++)
	if (newNameA[i] == 0)
	  printf (" ");
	else
	  printf ("%c", newNameA[i]);
    }

  start_time = clock ();
  rSendB (message, message_len, COMMAND_ECHO, DATA_ECHO, "Command");
  end_time = clock ();
  printf ("%3d", iterA);
  elapsed_time = (end_time - start_time) / (double) CLOCKS_PER_SEC;
  printf ("  %7.5lf  ", elapsed_time);


  start_time = clock ();
  rReceiveB (newNameB, numberOfBytes);
  end_time = clock ();
  printf ("%3d", iterB);
  elapsed_time = (end_time - start_time) / (double) CLOCKS_PER_SEC;
  printf ("  %7.5lf ", elapsed_time);

 if (printResult)
    {
      int i;
      for (i = 0; i < numberOfBytes; i++)
      for (i = 0; i < numberOfBytes; i++)
	if (newNameB[i] == 0)
	  printf (" ");
	else
	  printf ("%c", newNameB[i]);
    }

  int diffB = 0;

  int i;
  for (i = 0; i < numberOfBytes; i++)
    {
      if (newNameA[i] != newNameB[i])
	diffB++;
    }

  printf ("  %d\n", diffB);

  free (newNameA);
  free (newNameB);
}

int main ()
{

  printf ("program to test time and effort for different rSend and rReceive versions\n");
  //socket_num = rConnect ("/dev/tty.IPRE6-365877-DevB");
  socket_num = rConnect ("/dev/tty.IPRE6-365897-DevB");

  char onOff[2];
  printf ("print Bluetooth communications ('y' or 'n')? ");
  scanf ("%s", onOff);
  rSetBluetoothEcho (onOff[0]);

  char message[9];

  /* print heading of results */
  printf ("           rSendA        rReceiveA               rSendB        rReceiveB             Different\n");
  printf ("Command   iter.  time   iter.  time response    iter.  time   iter.  time response   from RecA\n");
  
  /* rGetName (bytes 0-7) */
  //Message 78 recieves 9 bytes of echo, and 8 bytes of message.
  message[0]= 78; // get first 8 bytes of the robot's 16-byte name
  message[1]= 0;  // not used
  message[2]= 0;  // not used
  message[3]= 0;  // not used
  message[4]= 0;  // not used
  message[5]= 0;  // not used
  message[6]= 0;  // not used
  message[7]= 0;  // not used
  message[8]= 0;  // not used

  printf ("getName1: "); 
  testMethods (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, 8, 1);

  /* rGetName (bytes 8-15) */
  message[0]= 64; // get last 8 bytes of the robot's 16-byte name
  message[1]= 0;  // not used
  message[2]= 0;  // not used
  message[3]= 0;  // not used
  message[4]= 0;  // not used
  message[5]= 0;  // not used
  message[6]= 0;  // not used
  message[7]= 0;  // not used
  message[8]= 0;  // not used
    
  printf ("getName2: "); 
  testMethods (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, 8, 1);

  /* rTakePicture) */
  message[0]= 64; // get last 8 bytes of the robot's 16-byte name
  message[1]= 0;  // not used
  message[2]= 0;  // not used
  message[3]= 0;  // not used
  message[4]= 0;  // not used
  message[5]= 0;  // not used
  message[6]= 0;  // not used
  message[7]= 0;  // not used
  message[8]= 0;  // not used
    
  printf ("rTakePic: "); 

  Picture pic1 = rTakePicture();
  rDisplayPicture (&pic1, 0, "pic1");
  Picture pic2 = rTakePicture();
  rDisplayPicture (&pic2, 0, "pic2");

  message[0] = 83;
  testMethods (message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, 192*256, 0);

  return 0;
}