/* program to make initial connection to Scribbler 2 robot
   after period of inactivity, when robot and workstation do not 
   communicate easily.
*/

#include "MyroC.h" 
#include <stdio.h> 
#include <stdlib.h> 
#include <ctype.h>
#include <string.h>
#include <unistd.h>
#include <termios.h>
#include <sys/socket.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <fcntl.h>
#include <sys/types.h>
#include <jpeglib.h>
#include <setjmp.h>
#include <signal.h>

int main ()
{
  printf ("program to handle initial communication between workstation\n");
  printf ("and Scribbler 2 robot via Bluetooth and \\dev\\rfcomm0\n");
  printf ("when initial connection difficult or after period of inactivity\n");

  char * address = "/dev/rfcomm0";
  int socket_num = open (address, O_RDWR | O_NOCTTY);
        
  // establish basic connection
  if (socket_num < 0)
    {
      fprintf (stderr, "error opening communication channel %s\n", address);
      exit (1);
    }

 // set terminal interface to control asynchronous communications ports
  const int baudrate = 38400;
  struct termios io;
  tcgetattr(socket_num, &io);
  cfsetospeed(&io, baudrate);
  cfsetispeed(&io, baudrate);
  cfmakeraw(&io);
  tcsetattr(socket_num, TCSANOW, &io);
  tcsetattr(socket_num, TCSAFLUSH, &io);

  // flush input and output
  tcflush(socket_num, TCIOFLUSH);
  //tcflush(socket_num, TCOFLUSH);
 
 /* ******************************************************************* */

  // ensure scribbler is not double-echoing by turning off echo mode
  char set_echo_message [9] = {98, 0, 0};
  int status = write (socket_num,  set_echo_message, 9);
  if (status != 9)
    {
      perror ("Error in establing connection over Bluetooth\n");
      exit (1);
    }

  // read echoed data
  int i, j;
  i = 0;
  while (i < 9)
    {
      char ch;
      j = read (socket_num, &ch, 1);
      if (j > 0)
        {
          printf ("message byte %i:  sent:  %d,  echo: %d\n", i, set_echo_message[i], ch);
          i += j;
        }
    }

  // clear sensor data
  i = 0;
  while (i < 11)
    {
      char ch;
      j = read (socket_num, &ch, 1);
      if (j > 0)
        {
          printf ("sensor data %i: echo: %d\n", i, ch);
          i += j;
        }
    }


   /* ******************************************************************* */
  printf ("executing zero beep\n");
  // beep at zero frequence for zero duration
  char zero_beep [9] = {113, 0, 0, 0, 0, -1, 6, 8, 9};
  status = write (socket_num, zero_beep, 9);
  if (status != 9)
    {
      perror ("Error in beginning Bluetooth synchronization\n");
      exit (1);
    }

  // read echoed data
  i = 0;
  while (i < 9)
    {
      char ch;
      j = read (socket_num, &ch, 1);
      if (j > 0)
        {
          printf ("message byte %i:  sent:  %d,  echo: %d\n", i, zero_beep[i], ch);
          i += j;
        }
    }

  // clear sensor data
i = 0;
  while (i < 11)
    {
      char ch;
      j = read (socket_num, &ch, 1);
      if (j > 0)
        {
          printf ("sensor data %i: echo: %d\n", i, ch);
          i += j;
        }
    }

  printf ("executing beep 880 Hz for 1 second\n");
  double duration = 1.0;
  int frequency = 880;
  char tone_beep [9] = {113, 
                        (((int)duration) * 1000) >> 8,
                        (((int)duration) * 1000) % 256,
                        frequency >> 8,
                        frequency % 256,
                        -1, 0, 0, 0};
  status = write (socket_num, tone_beep, 9);
  if (status != 9)
    {
      perror ("Error in beginning Bluetooth synchronization\n");
      exit (1);
    }

  // read echoed data
  i = 0;
  while (i < 9)
    {
      char ch;
      int j = read (socket_num, &ch, 1);
      if (j > 0)
        {
          printf ("message byte %i:  sent:  %d,  echo: %d\n", i, tone_beep[i], ch);
          i += j;
        }
    }


  /*
  printf ("executing soft reset\n");
  char soft_reset [9] = {33, 0, 0};
  status = write (socket_num, soft_reset, 9);
  if (status != 9)
    {
      perror ("Error in beginning Bluetooth synchronization\n");
      exit (1);
    }

  printf ("executing scribbler reset\n");
  char reset_scribbler [9] = {124, 0, 0};
  status = write (socket_num, reset_scribbler, 9);
  if (status != 9)
    {
      perror ("Error in beginning Bluetooth synchronization\n");
      exit (1);
    }
  */

  // close connection
  close (socket_num); 
  
  return 0;
}