/* * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-utilities-windows.c
 * @brief file with global variable declarations
 *
 *    helper functions for an individual user function are included in the
 *          implementation file for that function
 *
 * 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 
 *
 * Revisions for Version 3.2
 *    Henry M. Walker
 *    
 *    Practical range of rBeep duration identified as <= 3.0014 seconds
 *    Image display and processing resolves several matters and adds functionality
 *        function rDisplayPicture completely rewritten
 *                over time OpenGL rountines had encountered troubles
 *                on Linux, specifically glutHideWindow() hid images, but could not be
 *                          restored, with difficulties depending on the graphics card
 *                on Mac, High Sierra generated compile warnings and 
 *                          restricted threads that could display images
 *    function rDisplayImageProcessingErrors added
 *    function rWaitTimedImageDisplay updated substantially
 *
 * Revisions for Version 3.3
 *    Henry M. Walker
 *    
 *    Linux and Mac versions unchanged
 *    MyroC also ported to Windows 10
 *    
 *  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

 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
    rBeep2                        
    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, Linux, or Windows environment
         yielding three versions:
         MyroC-connect-mac.c
         MyroC-connect-linux.c
         MyroC-connect-windows.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                            rDisplayImageProcessingErrors (MyroC-display.c)
    rFastForward                        rWaitTimedImageDisplay(MyroC-display.c)
    rBackward                           rSavePicture (MyroC-image-file.c)
    rMotors                             rLoadPicture (MyroC-image-file.c)  
    rStop
    rHardStop

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

#include <winsock2.h> // needed for socket programming with Windows
#include <ws2bth.h>   // needed for Bluetooth with Windows
#include <stdio.h>    // needed for printing at terminal
#include <unistd.h>   // needed for read/write, usleep
#include <stdlib.h>   // needed for exit
#include <ctype.h>    // needed for tolower
#include <string.h>
#include "MyroC.h"
#include "MyroC-utilities.h"

/*
 ************************ global variables ******************************
 */

/* version specification 
 */
char * version = "MyroC.3.3";
int version_printed = 0;  // 0 - version not yet printed; 1 - version printed

// baudrate for Bluetooth communication
const int baudrate = 38400;

// debug flag for communications
int debug = 0;  // 1 == echo all communications; 0 no extra printing

// allow beeps and data exchange during opening connect
int followOpeningExchange = 1; // 1 == robot beeps, exchanges data at connect; 
                               // 0 == no beeps or data exchange

#include <pthread.h>
pthread_mutex_t bluetooth_lock;  // mutex to prevent concurrent communication over 
                                 //    socket for robot communication

/* 
 variables for counting motion commands per robot, based on socket number
 variables referenced in connections (rConnect, rDisconnect) and motion commands
 variable for whether or not to display error messages from image processes
 */

int current_num_robots = 0;

robot_motion_camera_record robot_sock_array [max_number_robots];

int displayImageErrors = 0;

/*
 ************************ utility functions  ******************************
 */

/*****************************************************************/
/* 0. UTILITY - UTILITY - UTILITY - UTILITY - UTILITY - UTILITY  */
/*****************************************************************/

/* Sends commands via bluetooth, assuming mutex set*/
/**
 * send bytes to the Scribbler --- same as rSend, EXCEPT
 * @pre                  mutex already 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 rSend_protected (char message[], int numberOfBytes,
                      int echo, int sensor_echo, char * commandText)
{
  /* send message */
   int debug = 0;

  int status = send (socket_num, message, numberOfBytes, 0);

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

  if (status != numberOfBytes)
    {
      perror ("Error in sending command over Bluetooth\n");
      printf ("    error number:  %d\n", errno);
      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 += recv (socket_num, command_echo+total_read,
                                numberOfBytes-total_read, 0);
         }

       /* 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, starting ");
                  fprintf (stderr, "at byte %d for %s command (code %d)\n",
                           i, commandText, message[0]);
                  break;
             }
           i++;
         }
       /* print command and echo 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]);
         }
    }

  if (sensor_echo)
    {
      int total_read = 0;
       char sensor_data [BYTES_OF_SENSOR_DATA];
      while (total_read < BYTES_OF_SENSOR_DATA)
        {
          total_read += recv (socket_num, sensor_data+total_read,
                              BYTES_OF_SENSOR_DATA-total_read, 0);
        }
       /* print sensor data in debug mode */
       if (debug)
         {
          printf("sensor data echoed:       byte(s), full value\n");

          printf("           IR left:      %3u (0=obstacle; 1=no obstacle))\n",
                  sensor_data [0]%256);
          printf("             rleft:      %3u (opposite spec. from rGetIR)\n",
                  sensor_data [1]%256);

          printf("        light left:      %3u  %3u,   %6u\n",
                  sensor_data[2], sensor_data[3],
                  (sensor_data[2]<<8 | sensor_data [3]));
          printf("            middle:      %3u  %3u,   %6u\n",
                  sensor_data[4], sensor_data[5],
                  (sensor_data[4]<<8 | sensor_data [5]));
          printf("             right:      %3u  %3u,   %6u\n",
                  sensor_data[6], sensor_data[7],
                  (sensor_data[6]<<8 | sensor_data [7]));

          printf("         line left:      %3u \n", sensor_data [8]);
          printf("             rleft:      %3u \n", sensor_data [9]);

          printf("             stall:      %3u \n", sensor_data [10]);

          /*
            int j;
            for (j = 0; j < BYTES_OF_SENSOR_DATA; j++)
            printf ("sensor data '%3u'\n", sensor_data[j]);
          */
         }
    }
}

/* Sends commands via bluetooth, assuming mutex NOT set*/
/**
 * send bytes to the Scribbler --- same as rSend_protected, EXCEPT
 * @pre                  a mutex has NOT been set to prevent concurrent 
 *                           Bluetooth processing
 * @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 rSend (char message[], int numberOfBytes, int echo, int sensor_echo, 
	    char * commandText)
{

  /* ensure only one message processed over Bluetooth at a time */
  pthread_mutex_lock (&bluetooth_lock);

  /* setting of mutex now meets pre-condition for rSend_protected */
  rSend_protected (message, numberOfBytes, echo, sensor_echo, commandText);

  /* release lock ensuring only one message over Bluetooth at a time */
  pthread_mutex_unlock (&bluetooth_lock);
}

/**
 * Receives data via bluetooth
 * @param message       a pointer to allocated space, where data will be stored
 * @param numberOfBytes number of bytes to be received from the Scribbler 
 * @pew                 space pointed to by message >= numberOfEchoBytes
 */
void rReceive (unsigned char * message, int numberOfBytes)
{
  /* ensure only one message processed over Bluetooth at a time */
  pthread_mutex_lock (&bluetooth_lock);

  int total_read = 0;
  while (total_read < numberOfBytes)
    {
          total_read += recv (socket_num, (char *) (message+total_read),
                              numberOfBytes-total_read, 0);
    }

  /* release lock ensuring only one message over Bluetooth at a time */
  pthread_mutex_unlock (&bluetooth_lock);
}

/**
 * @brief Turn on and off opening exchange with robot, including
 *                   distinctive beeps when connecting 
 *                   retrieval of robot name
 * @param onOff      char 'y' enables beeps (default)
 *                   char 'n' disables beeps 
 *                   other character values ignored
 */
void rSetOpeningExchange (char onOff)
{
    if (tolower (onOff) == 'y')
      followOpeningExchange = 1;
    else   if (tolower (onOff) == 'n')
      followOpeningExchange = 0;
    else  
      printf ("invalid parameter for enabling/disabling rconnect beeps\n");
}

/**
 * @brief Turn on and off echoing of Bluetooth transmissions
 * All robot commands involve the transmission of a command over Bluetooth
 *   Scribbler commands are always 9 bytes
 *   Fluke commands have varying lengths
 * The fluke echos most, but not all, of the commands
 * For many commands, the fluke also echos 11 bytes of sensor data
 * @param onOff      char 'y' enables echoing
 *                   char 'n' disables echoing
 *                   other character values ignored
 */
void rSetBluetoothEcho (char onOff)
{
  if (tolower (onOff) == 'y')
    debug = 1;
  else   if (tolower (onOff) == 'n')
    debug = 0;
  else  
    printf ("invalid parameter for echoing Bluetooth transmissions\n");
}

/**
 * @brief Turn on and off echo mode for Bluetooth communications
 * Echo Mode repeats the 1-byte command code after echoing the command
 *    and sensor data for Scribbler commands.
 * @param onOff      char 'y' enables echo mode (default)
 *                   char 'n' disables echo mode 
 *                   other character values ignored
 */
void rSetEchoMode (char onOff)
{
  char message [9];
  message [0] = 98;  /* set echo mode */
  message [2] = -1;
  message [3] = 0;
  message [4] = 0;
  message [5] = 0;
  message [6] = 0;
  message [7] = 0;
  message [8] = 0;

    if (tolower (onOff) == 'y')
      {
	message[1] = 1;
	printf ("turning echo mode on\n");
	rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rSetEchoMode");

      }
    else   if (tolower (onOff) == 'n')
      {
	message[1] = 0;
	printf ("turning echo mode off\n");
	rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rSetEchoMode");
      }
    else  
      {
	printf ("invalid parameter for enabling/disabling ");
        printf ("Bluetooth Echo Mode: %c\n", onOff);
      }
}


/**
 * @brief  determine Scribbler and Fluke versions, set default camera size
 * @param  printYorN   print Scribbler/Fluke version numbers during processing
 * @post   if original fluke, camera size set to 192 (height) by 256 (width)
 *         if Fluke 2,  camera size set to 800 (height) by 1280 (width)
 */
void rCheckHardwareVersionSetCameraSize (char printYorN)
{
  /* check there is a robot to connect to */
  if (current_num_robots == 0)
    {
      printf ("workstation not connected to a robot;\n");
      printf ("     hardware check not applicable\n");
      return;
    }
  /* robot is present; find robot in robot_sock_array */
  int robot_index = 0;
  while ((robot_index < current_num_robots)
         && (robot_sock_array [robot_index].robot_socket_num != socket_num))
    {
      robot_index++;
    }
  if (robot_index == current_num_robots)
    {
      printf ("workstation connection to robot not found;\n");
      printf ("     hardware check not available\n");
      return;
    }
  //printf ("socket number fouund with index:  %d\n", robot_index);
  /* send query for robot version data */
  char message [9];
  message [0] = 80;  /* GET_INFO command */
  message [1] = 0;
  message [2] = 0;
  message [3] = 0;
  message [4] = 0;
  message [5] = 0;
  message [6] = 0;
  message [7] = 0;
  message [8] = 0;
  rSend (message, 9, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "getFirmwareVersions");

  unsigned char response [200];
  int i = 0;

  rReceive (&response[i], 1);
  while (response[i] != '\n')
   {
     i++;
     rReceive (&response[i], 1);
    }
   response[i] = 0;

   /* maximum size of strings for version data */
   #define MAX_VERSION_STRING_SIZE 40

   /* strings for hardware/firmware data from robot */
   char fluke_str [MAX_VERSION_STRING_SIZE];
   char robot_str [MAX_VERSION_STRING_SIZE];
   char scribbler_str [MAX_VERSION_STRING_SIZE];
   char model_str [MAX_VERSION_STRING_SIZE];

   /* hardware/firmware versions */
   char fluke_ver [MAX_VERSION_STRING_SIZE];
   char robot_ver [MAX_VERSION_STRING_SIZE];
   char scribbler_ver [MAX_VERSION_STRING_SIZE];

   /* variables to unpack scribbler version information */
   char * version_token;

   /* unpack firmware data into separate strings */
   //printf ("here1, %d, %s\n", i, response);
   /* fluke firmware version number comes first */
   version_token = strtok ((char *) response, ",");
   strncpy (fluke_str, version_token, MAX_VERSION_STRING_SIZE);

   /* robot firmware version is second */
   version_token = strtok (NULL, ",");
   strncpy (robot_str, version_token, MAX_VERSION_STRING_SIZE);

  /* scribbler hardware version is third */
   version_token = strtok (NULL, ",");
   strncpy (scribbler_str, version_token, MAX_VERSION_STRING_SIZE);

   /* scribbler model is fourth */
   version_token = strtok (NULL, ",");
   strncpy (model_str, version_token, MAX_VERSION_STRING_SIZE);

   /* unpack fluke versions from fluke string and extract Fluke/Fluke 2 */
   char * fluke_token = strtok ((char *) fluke_str, ":");
   fluke_token = strtok (NULL, ":");
   strncpy (fluke_ver, fluke_token, MAX_VERSION_STRING_SIZE);

   switch (fluke_ver[0])
     {
     case '2':
       robot_sock_array [robot_index].camera_height = 192;
       robot_sock_array [robot_index].camera_width  = 256;
       robot_sock_array [robot_index].fluke_type = "original fluke";
       break;
       
     case '3':
       robot_sock_array [robot_index].camera_height = 266;
       robot_sock_array [robot_index].camera_width  = 427;
       robot_sock_array [robot_index].fluke_type = "Fluke 2";

       /* set Fluke 2 to low resolution */
       message[0] = 11; // code for setting Fluke 2 image size
       message[1] = 71; // code for small Fluke 2 image
       rSend (message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetCameraSize");

       /* delay to allowing processing of small image message;
	  required for Mac OS X processing */
      usleep (500000);
      /* turn white balance on */
      message [0] = 0x12;   // Fluke address for white balance
      message [1] = 0x14 | (1 << 2);  // code to turn balance on
      rSend (message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF,
             "rSetCameraSize-white balance");
      
       /* delay to allowing processing of white-balance message;
	  required for Mac OS X processing */
      usleep (100000);

       /* turn gain control and exposure control on */
       message [0] = 0x13;   // Fluke address for gain/exposure control
       message [1] = (char) (0xA3 | (1 << 1)  // code to turn gain control on
			          | (1 << 0));   // code to turn exposure control on
       rSend (message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF,
              "rSetCameraSize-gain/balance control");

       /* delay to allowing processing of gain/exposure message;
	  required for Mac OS X processing */
      usleep (100000);

       /* set parameters for Y, U, V configuration, 
          the smooth_thresh parameter (how many pizes must be black/white
          before the Run Length Encoding (RLE) recognizes the change
          this reduces minor noise, but the pictures may be slightly streaky
          a delay parameter must be 90 for internal timing
       */
       char yuv_message [9];          // store RLE/YUV parameters
       yuv_message [0] = 118;         // code to set YUV parameters
       yuv_message [1] =  90;         // delay required for internal fluke timing
       yuv_message [2] =   4;         // smooth threshhold
       yuv_message [3] =   0;         // low  Y value
       yuv_message [4] = (char) 200;  // high Y value
       yuv_message [5] =  51;         // low  U value
       yuv_message [6] = (char) 136;  // high U value
       yuv_message [7] = (char) 190;  // low  V value
       yuv_message [8] = (char) 255;  // high V value
       rSend (yuv_message, 9, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF,
              "rSetCameraSize-YUV configuration");

       /* set IR power level on Fluke */
       message [0] = 120;  // command code for SET_DONGLE_IR
       message [1] = 135;  // developer-recommended power level 
       rSend (message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF,
              "rSetCameraSize-setIR");
       break;
     
     default:
       robot_sock_array [robot_index].camera_height = 0;
       robot_sock_array [robot_index].camera_width  = 0;
       robot_sock_array [robot_index].fluke_type = "unrecognized";
       break;
     }

   /* print hardware/firmware versions, if requested */
   if (tolower (printYorN) == 'y')
     {
       /* unpack scribbler versions from scribbler and robot strings */
       char * scribbler_token = strtok ((char *) scribbler_str, ":");
       scribbler_token = strtok (NULL, ":");
       strncpy (scribbler_ver, scribbler_token, MAX_VERSION_STRING_SIZE);
   
       char * robot_token = strtok ((char *) robot_str, ":");
       robot_token = strtok (NULL, ":");
       strncpy (robot_ver, robot_token, MAX_VERSION_STRING_SIZE);

       printf ("    robot hardware:  %-12s firmware version:  %s\n",
               scribbler_ver, robot_ver);
       printf ("    fluke hardware:  %-12s firmware version:  %s\n",
               robot_sock_array [robot_index].fluke_type,
               fluke_ver);
       printf ("    camera image size (default):  %i (height) by %i (width)\n",
               robot_sock_array [robot_index].camera_height,
               robot_sock_array [robot_index].camera_width);
     }
}

/**
 * @brief return default image size from camera
 * @param width, height set to image size
 * @return "unknown", "original Fluke", "Fluke 2", or "unrecognized"
 * @post  if no connection to robot present, 
 *           dimensions are 0, 0; and "no connection" returned
 * @post  if previous connection, but type not determined, "unknown" returned
 * @post  if connection to robot, but no size specified previously,
 *           original fluke size:  192 by  256;  "original Fluke" returned
 *           Fluke 2 size:         800 by 1280;  "Fluke 2" returned
 * @post  if robot query not anticipated, "unrecognized" returned
 * @post  if connections previously set, dimensions returned as set
 */
const char * rGetCameraSize (int *height, int *width){
  /* camera data stord in struct for this robot within robot_soc_array */
  if (current_num_robots == 0)
    {
      *height = *width = 0;
      return "no connection";
    }
  else /* robot is present; find robot in robot_sock_array */
    {
      int robot_index = 0;
      while ((robot_index < current_num_robots)
             && (robot_sock_array [robot_index].robot_socket_num != socket_num))
        {
          robot_index++;
        }

      if (robot_index == current_num_robots)
        {
          *height = *width = 0;
          return "unknown";
        }

      *height = robot_sock_array [robot_index].camera_height;
      *width  = robot_sock_array [robot_index].camera_width;
      return robot_sock_array [robot_index].fluke_type;
    }
}

