/* * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-connect-linux.c  
 * @brief Linux-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

 ************************************************************************
 *
 *  This program and all MyroC software is licensed under the Creative Commons
 *  Attribution-Noncommercial-Share Alike 4.0 International License.
 *  Details may be found at http://creativecommons.org/licenses/by-nc-sa/4.0/
 *
 ************************************************************************
 */

#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 <bluetooth/bluetooth.h>
#include <bluetooth/rfcomm.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>

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

/**
 * connects program to Scribbler
 * @param address  string, giving name of workstation port
 *                 or a Scribbler Bluetooth designation
 *
 *                 several string formats are possible
 *                 a communications port, such as "/dev/rfcomm0"
 *                 a MAC address, such as "00:1E:19:01:0E:13"
 *                 a Scribbler 2 fluke serial number, such as "245787"
 *                 a full IPRE serial number, such as "IPRE245787"
 *                 a Fluke 2 serial number (hexadecimal), such as "021F"
 *                 a full Fluke 2 serial number, such as "Fluke2-021F"
 *
 * @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;
    }

  if (strchr(address, '/') > 0)
    { /* communications port identified */
      socket_num = open (address, O_RDWR | O_NOCTTY);
        
      if (socket_num < 0)
        {
          fprintf (stderr, "error opening communication channel %s\n",
                   address);
          exit (1);
        }
    }
 
  else 
    {
      /* if fluke or IPRE or Fluke 2 serial number, find MAC address */   
      char fluke_num [7] = {0};
      char mac_addr  [18] = {0};
      int endpos, i;
      /* if a fluke/IPRE number, last 6 characters will be digits
         string before digits either is empty or starts with IPRE 
         if a fluke2 number, then last 4 characters will be hex digits
         string before digits either is empty or starts with Fluke2-

      */

      /* first trim whitespace at end */
      endpos = strlen (address)-1;
      while (endpos >= 0 && isspace (address[endpos]))
        {
          endpos--;
        }
        
      /* determine if address is valid Fluke designator */
      /* if so, serial number stored in fluek_num array */
      int isValid = 0;

      /* first check if IPRE number (i.e., Fluke 1)
         last 6 characters must be decimal digits
         before number may be empty or contain "IPRE"
      */
      
      /* copy last 6 characters, if digits */
      for (i = 0; i < 6 && isdigit (address[endpos - 5 + i]); i++)
        fluke_num [i] = address[endpos - 5 + i];

      if (strlen(fluke_num) == 6 && 
          (endpos == 5 || strncmp ("IPRE", address, 4) == 0))
        isValid = 1;

      // printf ("test for IPRE number, result = %d\n", isValid);
      // printf ("   fluke_num: %s\n", fluke_num);

      if (!isValid)
        {
          for (i = 0; i < 4 && isxdigit (address[endpos - 3 + i]); i++)
            fluke_num [i] = address[endpos - 3 + i];
          fluke_num [4] = fluke_num [5] = 0;
          if (strlen(fluke_num) == 4 && 
              (endpos == 3 || strncmp ("Fluke", address, 5) == 0))
            isValid = 1;
        }

      // printf ("test for Fluke 2 number, result = %d\n", isValid);
      // printf ("   fluke_num: %s\n", fluke_num);

       /* now have an IPRE or Fluke 2 serial number 
             so look up MAC address */
      if (isValid)
        {
          /* code segment closely pattered from simplescan.c program
             from Bluethooth Essentials for Programmers by Huang & Rudolph */
             
          printf ("Using Bluetooth to find connection address for %s\n",
                  address);
          inquiry_info *devices = NULL;
          int max_rsp, num_rsp;
          int adapter_id, sock, len, flags;
          int i;
          char addr[19] = { 0 };
          char name[248] = { 0 };

          adapter_id = hci_get_route(NULL); 
          sock = hci_open_dev( adapter_id ); //open socket
          if (adapter_id < 0 || sock < 0)
            {
              perror("error opening socket");
              exit(1);
            }

        // initializations and memory allocation
        len  = 8;
        max_rsp = 255;
        flags = IREQ_CACHE_FLUSH;
        devices = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
    
        num_rsp = hci_inquiry(adapter_id, len, max_rsp, NULL, &devices, 
                              flags); //inquire the number of responding devices
        // printf ("number responses:  %d\n", num_rsp);
        if( num_rsp < 0 ) 
          { perror("error in Bluetoon hci_inquiry\n");
            exit (1);
          }
        // print name and address of each visible device
        for (i = 0; i < num_rsp; i++) 
          {
            ba2str(&(devices+i)->bdaddr,addr);//convert device address to string
            memset(name, 0, sizeof(name));
            hci_read_remote_name(sock, &(devices+i)->bdaddr, 
                                 sizeof(name), name, 0);
               
            //printf("%s  %s\n", addr, name);
              
            if (strstr (name, fluke_num) > 0)
              {
                strcpy (mac_addr, addr);
                break;
              }
          }

        // printf ("mac address found:  %s\n", mac_addr);

        //clean up
        free( devices );
        close( sock );

        //addr has MAC address, if Bluetooth name found
        if (strlen (fluke_num) == 0)
          {  fprintf (stderr, "Scribbler serial number %s not available\n", 
                      address);
            exit (1);
          } 
        }

    /* check possible machine address, if >= 2 colons in address */
    if ((strchr (address, ':') > 0) && 
        (strchr (address, ':') != strrchr (address, ':')))
      strcpy (mac_addr, address);

    /* cannot connect if MAC address not found */
    if (strlen (mac_addr) == 0)
      {
        fprintf (stderr, "cannot find Bluetooth channel for %s\n", address);
        exit (1);
      }

    /* MAC address found, so connection seems possible */
    printf ("     Machine (MAC) address for %s is %s\n", address, mac_addr);
    
    struct sockaddr_rc addr = { 0 };
    int status;
    
    // allocate a socket
    socket_num = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);

    // set the connection parameters (who to connect to)
    addr.rc_family = AF_BLUETOOTH;
    addr.rc_channel = 1;
    str2ba( mac_addr, &addr.rc_bdaddr );

    // connect to server
    status = connect(socket_num, (struct sockaddr *)&addr, sizeof(addr));
    if (status < 0)
      { perror ("error in establishing a connection\n");
        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);

  /* 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)
    {
      /* play opening note sequence */
      rBeep(.03, 784);
      rBeep(.03, 880);
      rBeep(.03, 698);
      rBeep(.03, 349);
      rBeep(.03, 523);      
      
      /* print greeting at terminal */
      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", address);

      /* query robot regarding versions
         for Fluke 2, set default camera size */
      rCheckHardwareVersionSetCameraSize ('y');

      printf ("return from rCheckHardwareVersionSetCameraSize\n");

      printf ("     ");   // indent forwardness message first time
      rSetForwardness("scribbler-forward");

    }

    /* 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);
    }
 
  printf ("connected on socket number:  %d\n", socket_num);
  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()
{

  // printf ("closing socket %d\n", socket_num);

  /* 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 ("robot found with array 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++;
    }

  char * robot_name = rGetName ();

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

  close (socket_num);

  // printf ("current number of open sockets:  %d\n", current_num_robots);
  
   /* 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);
}