#include <winsock2.h>
#include <ws2bth.h>
#include <stdio.h>
#include <ctype.h>
#include <string.h>
#include "MyroC.h"
#include "MyroC-utilities.h"

/* compile with
   gcc MyroC-connect-windows.c -lws2_32 -o MyroC-connect-windows.exe
*/
//#pragma comment(lib, "Ws2_32.lib");
#define BYTES_OF_SENSOR_DATA 11

// definitions for Bluetooth communications with sockets
SOCKET socket_num;
int err;
/* socket initialization variables---define if not already available from eSpeak */
#ifndef wVersionRequested
   WORD wVersionRequested = 0; // initialization to show no socket initialization yet
   WSADATA wsaData;
#endif 

SOCKADDR_BTH sockAddr;
int sockAddr_len = sizeof (sockAddr);
int error;

// specify structure for socket connection

#define INQUIRY_STR_MAX 20
struct bth_inquiry_record {
   char addrStr [INQUIRY_STR_MAX];
   char serviceName [INQUIRY_STR_MAX];
};

/* * * * * * * * * * * * *  printLastError * * * * * * * * * * 
 */
void printLastError()
{
   char lpMsgBuf[200] = {0};
   FormatMessage( FORMAT_MESSAGE_ALLOCATE_BUFFER |
                  FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
                  NULL, WSAGetLastError(), 0, (LPTSTR) &lpMsgBuf, 200, NULL );
   printf ("    '%s'\n", lpMsgBuf);
   printf ("    error code:  %d\n", WSAGetLastError());
   fflush(stdout);
}

/* * * * * * * * * * * * *  rConnectFullAddress * * * * * * * * * * 
 * @param   address, a string for the full Bluetooth
 *             for connecting to a robot
 * @pre     address has the form  XX:XX:XX:XX:XX:XX    
 * @pre     the robot address must be registered 
 *             as a Bluetooth device with the Windows computer
 * @pre    function called from rConnect, so that
 *            global sock variable initialed 
 *            only 1 socket created per call to rConnect
 *
 * @return  the socket number of communications port
 *
 * @post    subsequent communications will take place through
 *             this socket, unless changed by rSetConnection
 *
 * @note    since the port number of a robot is neither documented
 *             nor discoverable, all ports (1 through 31) are tried, in turn 
 */
int rConnectFullAddress (char * address) 
{
  /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  
   * connect to found device
   * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
  // set up sockAddr structure for Bluetooth connection 
   memset (&sockAddr, 0, sizeof (sockAddr));
   sockAddr.addressFamily = AF_BTH;
  
   // convert address string to Bluetooth address format
   if( SOCKET_ERROR == WSAStringToAddress( address, AF_BTH, NULL,
                                           (LPSOCKADDR) &sockAddr, 
                                           &sockAddr_len ) ) {
     printf ("error formatting Bluetooth address from '%s'\n", address);
     printLastError ();
     //exit (1);
   }

   // possible Bluetooth ports are numbered 1 to 31---try them all in sequence
   int port;
   for (port = 1; port <= 31; port++) {  
      //printf ("attempting connection on port %d\n", port);
      sockAddr.port = port;
      if (connect(socket_num, (SOCKADDR*)&sockAddr, sizeof (sockAddr))) {
         break;
      }
   }

   // check if no connection made
   if (port > 31) {
      printf ("error in connecting through socket to robot with address %s\n",
              address); 
      fflush(stdout);
      printLastError (); printf("   error processing done \n"); fflush(stdout);
      return -1;

   }

   // socket connection established
   printf ("Connection to robot established through address %s, port %d ", address, port);
   return 0;
}

/* * * * * * * * * * * * *  rConnect * * * * * * * * * *
 * @param     address, a string for connecting to a robot
 *              the specification of the robot follows these rules
 *              0.  non-hex leading and trailing characters are stripped
 *                     for all processing that follows 
 *              1.  if the full Bluetooth is given,
 *                  then the connection is made directly to that robot
 *                  note:  a complete Bluetooth address has the form
 *                      XX:XX:XX:XX:XX:XX
 *                     where X is a hexadecimal digit
 *              2.  if 4 hex digits (e.g., WXYZ) of a Fluke 2
 *                     serial number are given,
 *                  then the Fluke 2 prefix (i.e., B4:D8:A9:00)is added
 *                     to give B4:D8:A9:00:WX:YZ as the full Bluetooth address
 *              3.  if 5 decimal digits of an original Fluke serial number
 *                     are given,
 *                  then registered robot devices are searched and used
 *                     for a connection made
 *              4.  if none of the above are given, or if the above do
 *                     not yield a robot connect,
 *                  then the registered devices are presented, and
 *                     the user selects which robot to use
 * @pre       the full robot address must be registered
 *            as a Bluetooth device with the Windows computer
 *
 * @note      during processing any hex digit a=f is converted to upper case
 *
 * @return    the socket number of communications port
 *
 * @post      subsequent communications will take place through
 *            this socket, unless changed by rSetConnection
 */

SOCKET rConnect (const char * address)
{
   //printf ("Attempting to connect, based on address:  %s\n", address);
   //fflush (stdout);

   // strip leading and trailing non-hex digits
   int startIndex= 0;
   int endIndex = strlen(address) - 1;
   while ((startIndex <= endIndex) && (address[startIndex] != ':') 
           && (!isxdigit(address[startIndex])))
       startIndex++;
   while ((startIndex <= endIndex) && (address[endIndex] != ':') 
           && (!isxdigit(address[endIndex])))
       endIndex--;
   // extract stripped string, with hex digits (if found) in upper case
   char revAddress [endIndex-startIndex + 2];
   int start;
   for (start = 0; start <= endIndex; start++, startIndex++) {
      revAddress [start] = toupper(address[startIndex]);
   }
   revAddress[start] = 0;

   printf ("attempting to connect to robot on address:  '%s'\n", revAddress); 
   fflush(stdout);  

   /* socket startup ---but only if this step not already performed */
   if (wVersionRequested == 0) {
      wVersionRequested = MAKEWORD(2,2);
      err = WSAStartup(wVersionRequested, &wsaData);
      if (err != 0) {
         printf ("WSAStartup failed with error: %d\n", err); fflush(stdout);
         printLastError();
         exit (1);
      }
      //printf ("socket startup accomplished\n"); fflush(stdout);
   }

  /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *  
   * examine each connection option, in order  (see function header for details)
   * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
   int connection_result = -1;  // negative = no connection yet; non-negative = connection

   // create a socket---just one socket needed per call to rConnect
   socket_num = socket(AF_BTH, SOCK_STREAM, BTHPROTO_RFCOMM);
   if ( SOCKET_ERROR == socket_num) {
      printf ("error creating socket\n"); fflush(stdout);
      printLastError ();
      exit (1);
   }
 
   /* - - - - - option 1:  parameter gives full Bluetooth address - - - - - 
    * proper stripped address has form  XX:XX:XX:XX:XX:XX
    */
   if (strlen(revAddress) == 17) {
      int ok = 1;
      for (int idx1 = 0; idx1 < strlen(revAddress); idx1++) {
         if (idx1 % 3 == 2)
            ok = ok && (revAddress[idx1] == ':');
         else
            ok = ok && isxdigit(revAddress[idx1]);
      }
      if (ok) {
         connection_result = rConnectFullAddress (revAddress);
         if (connection_result >= 0) {
            printf ("using option 1\n");
         } else {
            printf ("no connection established == option 1\n");
         }  
      fflush(stdout); 
      }
   }

   /* - - - - - option 2:  parameter gives last 4 hex digits of Bluetooth address - - - - - 
    * proper stripped address has form  WXYZ, so full address is  B4:D8:A9:00:WX:YZ
    */
   if (strlen(revAddress) == 4) {
      int ok = 1;
      for (int idx2 = 0; idx2 < strlen(revAddress); idx2++) {
         ok = ok && isxdigit(revAddress[idx2]);
      }
      if (ok) {
         char fullAddr[18] = "B4:D8:A9:00:";
         fullAddr[12] = revAddress[0];
         fullAddr[13] = revAddress[1];
         fullAddr[14] = ':';
         fullAddr[15] = revAddress[2];
         fullAddr[16] = revAddress[3];
         fullAddr[17] = 0;
         strcpy(revAddress, fullAddr);
         printf ("trying to connect to address %s\n", revAddress);
         connection_result = rConnectFullAddress (revAddress);
         if (connection_result >= 0) {
            printf ("using option 2\n");
         } else {
            printf ("no connection established --- option 2\n");
         }
      }
   }

   /* - - - options 3 and 4 require a [time-consuming] search of available addresses - - - - 
    * in search, record address string and device name for each identified target
    */

   // try options 3 and/or 5, if connection not already established
   if (connection_result < 0) {

      // set up array to hold inquiry records
      int inq_rec_max_len = 10;
      int inq_rec_cur_len = 0;  //index of where to insert next record
      struct bth_inquiry_record * inquiry_rec_arr 
          = (struct bth_inquiry_record *)malloc(inq_rec_max_len*sizeof(struct bth_inquiry_record));

      // prepare the inquiry data structure
      DWORD qs_len = sizeof( WSAQUERYSET );
      WSAQUERYSET *qs = (WSAQUERYSET*) malloc( qs_len );
      ZeroMemory( qs, qs_len );
      qs->dwSize = sizeof(WSAQUERYSET);
      qs->dwNameSpace = NS_BTH;
      DWORD flags = LUP_CONTAINERS | LUP_FLUSHCACHE | LUP_RETURN_NAME
                                   | LUP_RETURN_ADDR| LUP_RETURN_TYPE;
      HANDLE h;

      // start the device inquiry
      if( SOCKET_ERROR == WSALookupServiceBegin( qs, flags, &h )) {
         fprintf (stdout, "error in starting WSA Lookup Service\n");
         printLastError ();
      }

      // iterate through the inquiry results
      int done = 0;
      while(! done) {
         if( NO_ERROR == WSALookupServiceNext( h, flags, &qs_len, qs )) {
            char buf[40] = {0};
            //BTH_ADDR result =
            //    ((SOCKADDR_BTH*)qs->lpcsaBuffer->RemoteAddr.lpSockaddr)->btAddr;
            DWORD bufsize = sizeof(buf);
            WSAAddressToString( qs->lpcsaBuffer->RemoteAddr.lpSockaddr,
                sizeof(SOCKADDR_BTH), NULL, buf, &bufsize);
            char * name = qs->lpszServiceInstanceName;
         
            if (buf[1] == 'B') {
               // expand inquiry array, if needed
               if (inq_rec_cur_len == inq_rec_max_len) {
                  //expand array
                  inq_rec_max_len *= 2;
                  struct bth_inquiry_record * temp_arr 
                     = (struct bth_inquiry_record *)malloc(inq_rec_max_len*sizeof(struct bth_inquiry_record));
                  // copy old inquiry array to new
                  for (int icp = 0; icp < inq_rec_cur_len; icp++) 
                    temp_arr[icp] = inquiry_rec_arr[icp];
                  free (inquiry_rec_arr);
                  inquiry_rec_arr = temp_arr;
               }
               strcpy (inquiry_rec_arr[inq_rec_cur_len].addrStr, buf);
               strcpy (inquiry_rec_arr[inq_rec_cur_len].serviceName, name);
               inq_rec_cur_len++;
            } 
         } else {
            int error = WSAGetLastError();
            if( error == WSAEFAULT ) {
               free( qs );
               qs = (WSAQUERYSET*) malloc( qs_len );
             } else if( error == WSA_E_NO_MORE ) {
                printf("inquiry process complete\n\n");
                done = 1;
             } else {
                printf("uh oh. error code %d\n", error);
                done = 1;
             }
         }
      }
 
      fflush(stdout);
      free( qs );

      /* - - - option 3 check available Fluke 1 serials in service name - - - 
       */
      if (strlen(revAddress) == 5) {
         int ok = 1;
         for (int idx2 = 0; idx2 < strlen(revAddress); idx2++) {
            ok = ok && isdigit(revAddress[idx2]);
         }
         if (ok) {
            // scan stored available addresses for address passed as parameter
            for (int idx3 = 0; idx3 < inq_rec_cur_len;  idx3++) {
                if (strstr(inquiry_rec_arr[idx3].serviceName, revAddress)) {
                   strcpy(revAddress, inquiry_rec_arr[idx3].addrStr);
                   printf ("trying to connect to address %s\n", revAddress);
                   connection_result = rConnectFullAddress (revAddress);
                   break;
                }
            }
            if (connection_result >= 0) {
               printf ("using option 3\n");
            } else {
               printf ("no connection established --- option 3\n");
            }
         }
      }

      /* - - - option 4 print robot options for user selection - - - 
       */
      if (connection_result < 0) {
         printf ("select among the following registered robots\n");
         printf ("    (be sure your desired robot is turned on)\n\n"); fflush(stdout);
      
         // print registered device information
         for (int idx4 = 0; idx4 < inq_rec_cur_len; idx4++) {
               printf("device %2d:  robot name: %s\n", idx4, inquiry_rec_arr[idx4].serviceName);
               printf("               address:  %s\n\n", inquiry_rec_arr[idx4].addrStr); fflush(stdout);
         }

        // retrieve desired device number
        printf ("enter desired robot/device number:  "); fflush(stdout);
        int option;
        scanf ("%d", &option);
        while (getchar() != '\n');  //clear line
        if ((option < 0) || (option >= inq_rec_cur_len)) {
           printf ("error:  device option must be between 0 and %d, inclusive", inq_rec_cur_len-1);
           return -1;
        }

        // connect to specified robot
        strcpy(revAddress, inquiry_rec_arr[option].addrStr);
        if (connection_result < 0) { 
           connection_result = rConnectFullAddress (revAddress);
           if (connection_result >= 0) {
              printf ("using option 4s\n");
           } else {
              printf ("no connection established --- no connection options successful\n");
              exit(1);
           }
        }
     }
   }   

   /* * * * record parameters for this connection, and record this socket * * * */
   /* 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 */
      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);

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

      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 ("    Connection process to %s completed\n", address);
   fflush(stdout);


   return socket_num;
}

/* * * * * * * * * * * * * * * * rDisconnect * * * * * * * * * * * * * */
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 %lld\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 = SOCKET_ERROR;
  robot_sock_array[i].motion_seq_num = 0;

  closesocket (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_robosts == 0) {
     socket_num = SOCKET_ERROR;
     wVersionRequested = 0;
     WSACleanup();  //release resources for Windows Sockets 2 API
  } 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_nsme);  
}