/* * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-connect-mac.c 
 * @brief Mac-based implementation file for Bluetooth connections/utilities
 *
 * Revision History
 *
 * Version 1.0 based on a C++ package by April O'Neill, David Cowden,
 *     Dilan Ustek, Erik Opavsky, and Henry M. Walker
 *
 * Developers of the C package for Linux:
 *  Creators Version 2.0 (C functions for utilities,general,sensors,movement):
 *    Spencer Liberto
 *    Dilan Ustek
 *    Jordan Yuan
 *    Henry M. Walker
 *  Contributors Version 2.2-2.3: (C functions for image processing)
 *    Anita DeWitt
 *    Jason Liu
 *    Nick Knoebber
 *    Vasilisa Bashlovkina
 * Revision for Version 2.4:  (image row/column made to match matrix notation)
 *    Henry M. Walker
 *
 * Revisions for Version 3.0 
 *    Henry M. Walker
 *
 *  C ported to Macintosh
 *     Linux/Mac differences required for connections — otherwise same code
 *  OpenGL used to display images, replacing ImageMagick
 *     same [new] code used for both Linux and Macintosh
 *     1 process for robot control
 *     1 process needed for each titled window (not each image, as in 2.2-2.4)
 *  Blocking options (negative duration parameter)
 *     utilize separate thread timer
 *  MyroC implementation files organized by user function as follows:
 *
 * Revisions for Version 3.1 
 *    Henry M. Walker
 *
 *    Picture struct and image functions revised to allow 
 *       192 by  256 images from origial Fluke camera
 *       266 x 427 low-resolution images from Fluke 2
 *         (high-resolution (800 x 1280) too large for more than 2-4 
 *          variables on run-time stack)
 *       storage , retrieval, and display of any images up to 266 x 427 
 *
 *  This program and all MyroC software is licensed under the Creative Commons
 *  Attribution-Noncommercial-Share Alike 3.0 United States License.
 *  Details available at http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 *
 ********************** implementation overview *************************

> indicates functions implemented in this file

 0. LOW-LEVEL UTILITY (prototypes in MyroC-utilities.h
                       implemented in MyroC-utilities.c)
    rSend
    rReceive
    rSetOpeningExchange
    rSetBluetoothEcho
    rSetEchoMode
    rCheckHardwareVersionSetCameraSize

 1. GENERAL (mostly MyroC-general.c) 2. SENSOR  (MyroC-sensors.c)
  > rConnect  (* MyroC-connect.c)     a.Scribbler Sensors
  > rDisconnect  (* MyroC-connect.c)    rGetLightsAll
    rSetConnection                      rGetLightTxt
    rFinishProcessing                   rGetIRAll
    rSetVolume                          rGetIRTxt
    rBeep                               rGetLine
    Beep2                        
    rSetName                          b.Fluke Sensors
    rGetName                            rGetObstacleAll
    rSetForwardness                     rGetObstacleTxt
    rGetForwardness                     rGetBrightAll
    rSetLEDFront                        rGetBrightTxt
    rSetLEDBack                         rSetIRPower
    rGetBattery                          
    rGetStall           

      Note:
      *  MyroC-connect requires special
         knowledge of a Mac or Linux
         environment, yielding two versions:
         MyroC-connect-mac.c
         MyroC-connect-linux.c

 3. MOVEMENT (MyroC-movement.c)      4. PICTURES (files as indicated)
    rTurnLeft                           rGetCameraSize (MyroC-camera.c)
    rTurnRight                          rTakePicture (MyroC-camera.c)
    rTurnSpeed                          rDisplayPicture (MyroC-display.c)
    rForward                            rWaitTimedImageDisplay(MyroC-display.c)
    rFastForward                        rSavePicture (MyroC-image-file.c)
    rBackward                           rLoadPicture (MyroC-image-file.c)
    rMotors                             
    rStop
    rHardStop

 ************************************************************************
 */

#include "MyroC.h" 
#include "MyroC-utilities.h"

#include <stdio.h> 
#include <stdlib.h> 
#include <ctype.h>
#include <string.h>
#include <fcntl.h>       // needed for open
#include <unistd.h>      // needed for close, read, write
#include <termios.h>     // needed to set Bluetooth communication parameters
#include <pthread.h>     // for threads, used in timing non-blocking images
#include <dirent.h>      // needed to check symbolic links in connection strins
#include <errno.h>       // needed to find error type when opening connection

/*
  @brief Check whether a string has a sequence of digits or hex digits
         To succeed, string must have exactly one sequence of a given length

  @param search_str    string to be searched
  @param target_len    string is searched for a char sequence of this length
  @param check_dig     function that characters in the sequence must satisfy
  @param address       address passed by user
  @return              'y' if search successful; 'n' if not
  @post                if successful, 
                         add '-' at start of search_str if number starts address
		         add '-' at end of search_str   if number ends address

  This procedure is used specifically by rConnect (twice)
*/
char check_digit_sequence (char * search_str, int target_len,
                           int check_dig (int c),
			   const char * address)
{
  // printf ("\naddress string:  %s; initial search string:  %s\n",
  //          address, search_str);

  /* initialiaation
  */
  int num_start = -1;          // starting index of longest found sequence
  int temp_num_start = -1;     // starting index of current found sequence
  int max_num_len = 0;         // length of longest found sequence
  int cur_num_len = 0;         // length of current found seequence
  int i;
  char pattern_found = '?';

  /* progress through string character-by-character
   */
  for (i = 0; i < strlen (search_str); i++)
    {
      if (check_dig (search_str[i]))
	{ // process digit
	  if (temp_num_start == -1)
	    {
	      temp_num_start = i;
	    }
	  cur_num_len++;
	}
      else
	{ // no digit - update max-digit-string records
	  if (max_num_len < cur_num_len)
	    {
	      max_num_len = cur_num_len;
	      num_start = temp_num_start;
	      temp_num_start = -1;
	      cur_num_len = 0;
	    }
	  else if ((max_num_len == cur_num_len)
		   && (max_num_len == target_len))
	    {  // two six-digit decimal numbers - invalid 
	      // printf ("invalid connection string: '%s' A\n", address);
	      pattern_found = 'n';
	      break;
	    }
	}
    }

  //  printf ("search loop done with max_num_len: %d, cur_num_len: %d\n",
  //	  max_num_len, cur_num_len);

  /* check if number at end of string */
  if (max_num_len < cur_num_len)
    {
      // longest sequence at end of string
      if (cur_num_len == target_len)
	{ // number at end, so add '-' at end of search string
	  search_str [i] = '-';
	  search_str [i+1] = 0;
	  pattern_found = 'y';
	  num_start = temp_num_start;
	}
      else
	{ // longest sequence has wrong length
	  // printf ("invalid connection string: '%s' %d B\n",
          //         address, max_num_len);
	  pattern_found = 'n';
	}
    }
  else if (max_num_len != target_len)
    { // no target numbers - invalid 
      // printf ("invalid connection string: '%s' %d C\n",
      //         address, max_num_len);
      pattern_found = 'n';
    }
  else if  (max_num_len == cur_num_len)
    { // two target numbers - invalid 
      // printf ("invalid connection string: '%s' %d D\n",
      //         address, max_num_len);
      pattern_found = 'n';
    }
  else
    { // valid number found
      pattern_found = 'y';
    }

  /* if target number starts string, add '-' at start */
  if ((pattern_found == 'y') && (num_start == 0))
    { // insert '-' at start of string
      for (i = strlen (search_str); i >= 0; i--)
	{
	  search_str [i+1] = search_str [i];
	}
      search_str [0] = '-';
    }
  // printf ("target-digit search_str:  %s\n", search_str);

  return pattern_found;
}

/*****************************************************************/
/* 1. GENERAL - GENERAL - GENERAL - GENERAL - GENERAL - GENERAL  */
/*****************************************************************/

/**
 * @param address  string, giving name of workstation port
 *                 or a Scribbler Bluetooth designation
 *
 *                 several string formats are possible
 *                 a complete device file name, such as 
 *                     + "/dev/rfcomm0" 
 *                     + "/dev/tty.IPRE6-365877-DevB"
 *                     + "/dev/tty.Fluke2-0958-Fluke2"
 *                 any substring of a complete device file name,
 *                     as long as the resulting device is unique
 *                     some possibilities include                               
 *                     + a Scribbler 2 fluke serial number, such as "245787"
 *                     + a full IPRE serial number, such as "IPRE6-245787"
 *                     + a Fluke 2 serial number (hexadecimal), such as "021F"
 *                     + a full Fluke 2 serial number, such as "Fluke2-021F"  
 *                     uniqueness is ensured by requiring 4 hex digits 
 *                         or 6 decimal digits
 *                 the full path of a symbolic link to a device file name string
 *                     or substring in /dev 
 *
 * Note:   Connections via a MAC address, such as "00:1E:19:01:0E:13", is 
 *         NOT available, for MyroC on Mac OS X, since AF_BLUETOOTH socket 
 *         types are not defined in /usr/include/sys/socket.h
 *         Type man socket for additional information
 *                                                           
 * @return         if a connection is made 
 *                    the socket number of communications port is returned
 *                 otherwise, the program exits, as robot processing cannot continue
 *
 * @post           subsequent communications will take place through
 *                 this socket, unless changed by rSetConnection
 */
int rConnect (const char * address)
{
  /* check number of current connections does not exceed maximum */
  if (current_num_robots >= max_number_robots)
    {
      printf ("already at limit of %d concurrent robot connections\n", 
        max_number_robots);
      printf ("   connection request denied\n");
      return -1;
    }

  /* working_address:  string used for searching given address
     if address is a symbolic link, working_address is what address points to
  */
  char * working_address;

  // sym_link array is hopefully long enough for full path
  //     of symbolic link, if any
  int size_sym_link_array = 100;
  char sym_link [size_sym_link_array];

  int sym_size = readlink (address, sym_link, size_sym_link_array);
  if (sym_size > 0)
    {
      printf ("Connection string %s is a symbolic link\n", address);
      sym_link [sym_size] = 0;   // place null character at end of string
      working_address = malloc (sizeof (char) * (strlen(sym_link) + 1));
      strcpy (working_address, sym_link);
      printf ("   exploring possible connection using reference link:  %s\n",
              sym_link);
    }
  else 
    {
      working_address = malloc (sizeof (char) * (strlen(address) + 1));
      strcpy (working_address, address);
      // printf ("%s is not a symbolic link\n", address);
    }

  /* strings for results of /dev directory search */
  char * connection_str = "";  // full path of device file for connection
  char * fluke_type = "";      // indicates Fluke or Fluke 2, when known

  /* construct search string to compare with devices of the form /dev/tty.*
     allow addition of - character at front or rear of string, as needed 
     to ensure uniqueness
   */

  char * search_str = malloc (sizeof (char) * strlen (working_address) + 3);
  if (working_address == NULL)
     strcpy (search_str, "");
  else
     strcpy (search_str, working_address);
  
  char * pos;
  char device_found = 'n';  /* 'n' address not found; 
                               'y' address found; 
                               'i' address invalid */

  //  printf ("\n-->address:  %s  working_address:  %s  search_str:  %s\n", 
  //	  address, working_address, search_str);

  /*  check if string contains a period .  */
  pos = strchr (working_address, '.');
  if (pos != NULL)
     {  /* prefix of string must be period-terminated substring of /dev/tty. */
        /* copy prefix to search string to ensure correct form */
        strncpy (search_str, working_address, pos-working_address);
               //search string contains prefix
        search_str[pos-working_address+1] = 0;
        if (strstr ("/dev/tty.", search_str) == NULL)
           { // invalid prefix ==> address not well formed
             // printf ("connection string '%s' has an invalid prefix\n",
             //          working_address);
             device_found = 'i';
           }
        else
           {
            strcpy (search_str, pos+1); // remove prefix from search string
           }
     }

  /* check address for a unique 6-digit decimal or 4-digit hex decimal nymber */
  if (device_found != 'i')
    {
      if (check_digit_sequence (search_str, 6, isdigit, working_address) == 'y')
	{
	  fluke_type = "Fluke ";
	}
      else if (check_digit_sequence (search_str, 4, isxdigit, working_address)
               == 'y')
	{
	  fluke_type = "Fluke 2 ";
	}
      else 
	{
	  device_found = 'i';
	  // printf ("valid digit sequence not found\n");
	}
    }

  DIR *dir_ptr;
  struct dirent *dir_entry;

  /* if possibly valid string, check if device file in /dev directory */
  if ((working_address != NULL) && (device_found != 'i'))
     {
        dir_ptr = opendir ("/dev");

        /* look for search_str within /dev filename */
        if (dir_ptr != NULL)
	  { 
	    while ((device_found != 'y') 
		   && ((dir_entry = readdir (dir_ptr)) != NULL))
	      {
		if ((strstr (dir_entry->d_name, search_str) != NULL)
		    && (strstr (dir_entry->d_name, "tty.") != NULL))
		  {
		    connection_str 
		      = malloc (sizeof (char)*(strlen (dir_entry->d_name) + 5));
		    strcpy (connection_str, "/dev/");
		    strcat (connection_str, dir_entry->d_name);
		    //printf ("full path name found:  %s\n", connection_str);
		    device_found = 'y';
		    break;
		  }
	      }
	  }
	closedir (dir_ptr);
     }

  if (device_found != 'y')
    {// device not found, so print possibilities

      printf ("Bluetooth framework not available for %s\n", working_address);
      printf ("    choose from the follow available options\n");

      dir_ptr = opendir ("/dev");
      
      if (dir_ptr != NULL)
	{ // print valid device options
	  printf ("device   Fluke serial number\n");

	  int dev_num = 0;
	  while ((dir_entry = readdir (dir_ptr)) != NULL)
	    {
	      /* check "tty." at start of string */
	      if (strstr(dir_entry->d_name, "tty.") == dir_entry->d_name)
		{  if  ((strcasestr(dir_entry->d_name, "IPRE") != NULL)
			|| (strcasestr(dir_entry->d_name, "Fluke") != NULL))
		    {
		      char * name
                        = malloc (sizeof(char) * (strlen(dir_entry->d_name)+1));
		      strcpy (name, dir_entry->d_name);
		      /* find last '-' */
		      char * last_pos = strrchr (name, '-');
		      if (last_pos == NULL)
			{
			  printf ("'-' not found in %s\n", name);
			}
		      else
			{ int abbr_len = last_pos - name - 4;
			  char * abbr_name = malloc (sizeof(char)* abbr_len+6);
			  strncpy (abbr_name, name+4, abbr_len);
			  printf ("%4d:  '%s'\n", dev_num, abbr_name);
			  dev_num++;
			  free (abbr_name);
			}
		      free (name);
		    }
		}
	    }
	  closedir (dir_ptr);

	  printf ("   enter number for desired Bluetooth device:  ");
	  int option;
	  do {
	    scanf ("%d", &option);
	    if ((option < 0) || (option >= dev_num))
	      printf ("option must be between 0 and %d --- try again;  ",
		      dev_num-1);
	  } while ((option < 0) || (option >= dev_num));

	  /* retrieve device file string */
	  dir_ptr = opendir ("/dev");
	  int i = 0;
	  while (i <= option)
	    {
	      dir_entry = readdir (dir_ptr);
	      /* check "tty." at start of string */
	      if (strstr(dir_entry->d_name, "tty.") == dir_entry->d_name)
		{  if  ((strcasestr(dir_entry->d_name, "IPRE") != NULL)
			|| (strcasestr(dir_entry->d_name, "Fluke") != NULL))
		    { // valid Fluke/Fluke 2 device found
		      i++;
		    }
		}
	    }
	  connection_str = malloc (sizeof(char)*(strlen(dir_entry->d_name)+5));
	  strcpy (connection_str, "/dev/");
	  strcat (connection_str, dir_entry->d_name);
	  device_found = 'y';
	  closedir (dir_ptr);
	}
      else
	{
	  printf ("/dev directory for Scribbler Bluetooth devices not found\n");
	  exit (1);
	}
    }

  if (device_found == 'y')
    {// device found, so make connection
      printf ("Establishing %sconnection using '%s'\n",
              fluke_type, connection_str);
    }
  else
    {
      printf ("Valid device not found\n");      
      exit (1);
    }

  /* cleanup temporary memory allocations */
  if (working_address != NULL)
    free (working_address);
  if (search_str != NULL)
    free (search_str);

  /* Bluetooth device file found --- open it */
  socket_num = open (connection_str, O_RDWR | O_NOCTTY);

  if (socket_num < 0)
    {
      fprintf (stderr, "error opening communication channel %s:  %s\n",
	       connection_str, strerror (errno));
      exit (1);
    }

  // set terminal interface to control asynchronous communications ports
  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);

  /* Echo Mode repeats the 1-byte command code after echoing the command
     and sensor data for Scribbler commands.
     By default Echo Mode should be off */
  //rSetEchoMode ('n');

   /* record Fluke and camera information, initially unknown */
  robot_sock_array [current_num_robots].camera_height = 0;
  robot_sock_array [current_num_robots].camera_width = 0;
  robot_sock_array [current_num_robots].fluke_type = "unknown";

  /* record motion command sequence number for new robot connection */
  robot_sock_array [current_num_robots].robot_socket_num = socket_num;
  robot_sock_array [current_num_robots].motion_seq_num = 0;

  /* no motion timer currently active for this robot */
  robot_sock_array [current_num_robots].thread_motion_timer_id = 0;
  current_num_robots++;
  
 /* sound distinctive opening tones */
  if (followOpeningExchange)
    {
      rBeep(.05, 784);
      rBeep(.05, 880);
      rBeep(.05, 698);
      rBeep(.05, 349);
      rBeep(.05, 523);
      
      /* print greeting at terminal */
      const char * robot_name = rGetName ();
      if (version_printed == 0)
        {
          version_printed = 1;
          printf ("Hello, I'm %s, running %s\n", robot_name, version);
        }
      else
        {
          printf ("Hello, I'm %s\n", robot_name);

        }
      free (robot_name);
      
      printf ("     connection established to %s\n", connection_str);

      /* query robot regarding versions

         for Fluke 2, set default camera size */
      rCheckHardwareVersionSetCameraSize ('y');
      
      printf ("     ");   // indent forwardness message first time
      rSetForwardness("scribbler-forward");
    }

  /* connection string no longer needed */
  free (connection_str);

/* set up mutex lock, so Bluetooth communication for the Scribbler
     does not interfere with communication from another thread
  */
  if (pthread_mutex_init (&bluetooth_lock, NULL) != 0)
    {
      printf ("\nBluetooth mutex initialization failed\n");
      exit (1);
    }
  
  return socket_num;
}

/**
 * @brief stop Scribbler motion and close Bluetooth
 *
 * @post                  motion for the current robot is stopped,
 *                           blocking until any non-blocking motion time
 *                           has expired                                      \n
 *                           i.e., if a motion timer is set,                  \n
 *                                    this procedure blocks, then stops motion\n
 *                                 else, procedure stops motion immediately   \n
 *                        Bluetooth for the current robot is closed
 */
void rDisconnect()
{

  /* find socket number in array robot_sock_array */
  int i;
  for (i = 0; i < current_num_robots; i++)
     {
       //printf ("searching robot %d with socket number %d\n",
       //         i, robot_sock_array[i].robot_socket_num);
       
        if (robot_sock_array[i].robot_socket_num == socket_num)
            break;
     }

  if (i >= current_num_robots)
    {
       printf ("cannot disconnect:  no robot associated with number %d\n",
               socket_num);
       return;
    }

  //printf ("socket found in array with index %d\n", i);

  /* if motion timer active, wait for it to finish */
  if (robot_sock_array[i].thread_motion_timer_id != 0)
    {
      printf ("motion timer active for this robot; waiting for timer\n");
      pthread_join (robot_sock_array[i].thread_motion_timer_id, NULL);
      printf ("motion timer processed\n");
    }

  else
    {
      /* stop any on-going robot motion */
      rMotors (0.0, 0.0);
    }

  /* remove record of robot from robot_sock_array */
  current_num_robots--;
  while (i < current_num_robots)
    {
       robot_sock_array[i] = robot_sock_array[i+1];
       i++;
    }

  const char * robot_name = rGetName ();

  robot_sock_array[i].robot_socket_num = -1;
  robot_sock_array[i].motion_seq_num = 0;

  close (socket_num);

  /* set active robot to last one connected with rConnect*/
  if (current_num_robots == 0)
    socket_num = -1;
  else
    socket_num = robot_sock_array[current_num_robots -1].robot_socket_num;

  printf ("Robot %s stopped and Bluetooth connection closed\n",
	  robot_name);
  free (robot_name);
  
}