/* * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-general.c 
 * @brief Implementation file for general Scribbler 2 procedures
 *
 * 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/
 *
 * 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
 *
 *    MyroC ported to Windows 10, in addition to Linux and Mac OS X
 *    Additional refinements
 *       text parameter for rGetLightTxt expanded to "Middle" as well as "Center"
 *
 *  This program and all MyroC software is licensed under the Creative Commons
 *  Attribution-Noncommercial-Share Alike 3.0 United States License.
 *  Details may be found at http://creativecommons.org/licenses/by-nc-sa/3.0/us/
 *
 *  
 ********************** implementation overview *************************

> indicates functions implemented in this file

 0. LOW-LEVEL UTILITY (defined in MyroC-utilities.h
                       implemented in * MyroC-connect.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
  > rBeep2                        
  > rSetName                          b.Fluke Sensors
  > rGetName                            rGetObstacleAll
  > rSetForwardness                     rGetObstacleTxt
  > rGetForwardness                     rGetBrightAll
  > rSetLEDFront                        rGetBrightTxt
  > rSetLEDBack                         rSetIRPower
  > rGetBattery                          
  > rGetStall                        
   
    * 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

 ************************************************************************
 */
/* Note: There is an r in the beginning of every function to make it 
   easier to understand whether it is a robot function. */

#include "MyroC.h" 
#include "MyroC-utilities.h"
#include <stdio.h> 
#include <stdlib.h> 
#include <ctype.h>
#include <string.h>
#include <unistd.h>    // needed for usleep

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

/**
 * set current connection to the socket number
 * @param new_socket_num  the number of an open socket for communication
 * @pre                   new_socket_num has been returned by rConnect
 *                        the designated socket has not been closed
 */
void rSetConnection (ROBOT_CONNECTION_TYPE new_socket_num)
{ 
  /* ensure only one message processed over Bluetooth at a time */
  pthread_mutex_lock (&bluetooth_lock);

  socket_num = new_socket_num;

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

/**
 * @brief                 all timed motions/image displays completed, all robots
 *                           stopped, and all robot Bluetooth connections closed
 *
 * @brief                 produces same result as calling rDisconnect for all robots
 *                        and also calling rComleteImageDisplay
 *
 * @post                  procedure blocks until all timed robot motions are complete,
 *                           and all timed image displays are no longer visible
 *                        upon completion, all timed image displays are closed,
 *                           all robot motion is halted, and
 *                           all robot Bluetooth connections closed
 */
void rFinishProcessing ()
{
   /* close all timed, non-blocking image displays */
   rWaitTimedImageDisplay ();

   /* stop all robots and close Bluetooth connections */
   int i;
   for (i = current_num_robots-1; i >= 0; i--)
      {
          socket_num = robot_sock_array [i].robot_socket_num;
          rMotors(0.0, 0.0);
          rDisconnect ();
      }
}

/**
 * @brief Set sould to high volume (H) or mute (M)
 * @parm highMute   set volume of Scribbler
 * @pre             highMute is 'H' Oto set for high volume
 *                  or 'M' for mute
 */

void rSetVolume (char highMute)
{
  char message [9];
  highMute = tolower (highMute);
  if (highMute == 'h')
    {
      message [0] = 111;   // Robot works in loud mode (high volume)
    }
  else
    {
      message [0] = 112;   // Robot's speaker is muted
    }
  message [1] = -1;
  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_ON, ECHO_SENSOR_ON, "rSetVolume");

  usleep (200000);
}

/**
 * @brief Beeps with the given duration and frequency
 * @param duration   length of note in seconds
 * @param frequency  frequency of pitch in cycles per second (hertz)
 * @pre              0.0 < duration > 0.0 <= 3.0 seconds
 *                   (Scribbler 2 hangs if duration > 3.0014 seconds;
 *                    although reason unknown,
 *                    experimentation suggests part of Sribbler 2 firmware)
 */
void rBeep(double duration, int frequency)
{
  char message[9];

  message[0]=113; // set speaker
  message[1]= (unsigned char) (((int)(duration * 1000))  >> 8);    // high-order 8 bits
  message[2]= (unsigned char) (((int)(duration * 1000))   % 256);  // low-order 8 bits
  message[3]= (unsigned char) (frequency >> 8); // high-order 8 bits
  message[4]= (unsigned char) (frequency % 256);// low-order 8 bits
  message[5]= -1 ; // end of data set
  message[6]= 0;   // not used
  message[7]= 0;   // not used
  message[8]= 0;   // not used

  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rBeep");

  usleep (2000); // delay 0.0005 seconds to facilitate thread timing 
}

/**
 * @brief Generates two notes for the prescribed duration
 * @param duration  length of note in seconds
 * @param freq1     frequency of first pitch in cycles per second (hertz)
 * @param freq2     frequency of second pitch in cycles per second (hertz)
 * @pre             0.0 < duration <= 3.0 seconds
 *                  (Scribbler 2 hangs if duration > 3.0014 seconds;
 *                   although reason unknown,
 *                   experimentation suggests part of Sribbler 2 firmware)
 */
void rBeep2(double duration, int freq1, int freq2)
{
  char message[9];

  message[0]=114; // set speaker
  message[1]= (unsigned char) (((int)(duration * 1000)) >> 8);   // high-order 8 bits
  message[2]= (unsigned char) (((int)(duration * 1000)) % 256);  // low-order 8 bits
  message[3]= (unsigned char) (freq1 >> 8);  // high-order 8 bits
  message[4]= (unsigned char) (freq1 % 256); // low-order 8 bits
  message[5]= (unsigned char) (freq2 >> 8) ; // high-order 8 bits
  message[6]= (unsigned char) (freq2 % 256); // low-order 8 bits
  message[7]= -1;   // end of data set
  message[8]= 0;    // not used
     
  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rBeep2");
}

/**
 * Change name stored in the robot to the 16-byte name given
 * @param name  specifies new name of robot
 *              if < 16 bytes given, name is filled with null characters
 *              if >= 16 bytes given, name is truncated to 15 bytes plus null
 */
void rSetName (const char * name)
{
  char message[9];
  int strlength = strlen(name);
  int i;

  message[0]= 110; // set first 8 bytes of the robot's 16-byte name
  for (i = 0; i < 8; i++)
    if (i < strlength)
      message [i+1] = name[i];
    else 
      message [i+1] = 0;
 
  //first part of scribbler name being set
  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rSetName (bytes 0-7)");
  // printf ("first 8 bytes set\n");

  message[0]= 119; // set first 8 bytes of the robot's 16-byte name
  for (i = 8; i < 16; i++)
    if (i < strlength)
      message [i-7] = name[i];
    else 
      message [i-7] = 0;

  //last part pf scribbler name being set
  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rSetName (bytes 8-15)");
  // printf ("second 8 bytes set\n");
}

/**
 * @brief Get the name of the robot
 * @return information about the name of the robot
 * @post  the returned name is a newly-allocated 17-byte string
 */
char * rGetName ()
{
  char message[9];
  unsigned char * newName = (unsigned char *) malloc(sizeof(char)*17);

  //Message 78 recieves 9 bytes of echo, and 8 bytes of message.
  message[0]= 78; // get first 8 bytes of the robot's 16-byte name
  message[1]= 0;  // not used
  message[2]= 0;  // not used
  message[3]= 0;  // not used
  message[4]= 0;  // not used
  message[5]= 0;  // not used
  message[6]= 0;  // not used
  message[7]= 0;  // not used
  message[8]= 0;  // not used
 
  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetName (bytes 0-7)");
  rReceive (newName, 8);

  newName[8] = '\0';
  //printf("%s\n", newName);

  message[0]= 64; // get last 8 bytes of the robot's 16-byte name
  message[1]= 0;  // not used
  message[2]= 0;  // not used
  message[3]= 0;  // not used
  message[4]= 0;  // not used
  message[5]= 0;  // not used
  message[6]= 0;  // not used
  message[7]= 0;  // not used
  message[8]= 0;  // not used
    
  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetName (bytes 8-15)");
  rReceive (newName+8, 8);
  newName[16] = '\0';

  return (char *) newName;
}

/**
 * specifies which end of the Scribbler is considered the front
 * @param direction  identifies front direction
 * @pre              direction is either "fluke-forward" or "scribbler-forward"
 *                   (not case sensitive)
*/
void rSetForwardness (char * direction)
{
  int len = strlen(direction) + 1;
  char workingString [len];
  int i;
  for (i = 0; i < len; i++)
    workingString[i] = tolower (direction[i]);

  char message[2];
  message[0]=128; 

  if (strcmp(workingString, "fluke-forward") == 0)
    {
      /* code for fluke forward is 0x80; */
      message[1] = (unsigned char) 0x80;
      printf ("setting fluke forward\n");
    }
  else if (strcmp (workingString, "scribbler-forward") == 0)
    {
      /* code for Scribbler forward is 0 */
      message[1] = (unsigned char) 0;
      printf ("setting scribbler forward\n");
    }
  else
    {
      fprintf (stderr, "rSetForwardness: unknown direction. Give either fluke-forward or scribbler-forward");
      return;
    }
     
  rSend (message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetForwardness");
}

/**
 * alternative to rSetForwardness for compatibility with earlier MyroC 
 */
void rSetForwardnessTxt (char * direction)
{
  rSetForwardness (direction);
}

/**
 * Gets the forwardness of the Scribbler
 * @return either "fluke-forward" or "scribbler-forward"
 */
char * rGetForwardness ()
{
  char message[5];
  char * result;

  message[0]= 90;//read_mem(0,0) 
  message[1]= 0; //page (2 bytes)
  message[2]= 0;
  message[3]= 0; //offset (2 bytes);
  message[4]= 0;
     
  rSend (message, 5, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rGetForwardness");
  unsigned char returned = 7;
  rReceive (&returned, 1);

  char flag = (unsigned char) 0xDF;
  //printf("rGetForwardness %x\n", returned);
  
  if( returned == flag) 
    result = "scribbler-forward";
  else
    result = "fluke-forward";

  return result;
}

/**
 * Set the front [fluke] LED on or off
 * @param led  value 1 turns on LED
 *             value 0 turns off LED
 * @pre        led must be 0 or 1
 */
void rSetLEDFront(int led){
  char message[1];
  if( led >= 1)
    {
      message[0]= 116; // turn the led on
      rSend(message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "Turn Front LED ON");
    }
  else
    {
      message[0]= 117; // turn the led off
      rSend(message,1,ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "Turn Front LED OFF");
    }
}

/**
 * Set the the intensity of the back fluke LED,
 * @param led  intensity of the LED
 *             values between 0 and 1 provide a range of brightness
 *             from off to full intensity
 *             values bigger than 1 are treated as 1 (full brightness)
 *             values less than 0 are treated as 0 (LED off).
 */
void rSetLEDBack(double led)
{
  char value = 0;

  if (led >=  1.0)
    {
      value = 255; 
    }
  else if (led >= 0.0 && led < 1.0 )
    {
      // convert led into an int for message      
      // scribbler does not display light for values below 140.
      // Use a linear scale between 140-255
      value = (char) (led  * 115.0 + 140.0);
    }
  else if (led < 0.0)
    value = 0;
  char message[2];
  message[0]= 126;   // turn the leds on
  message[1]= value; // testing int
  rSend(message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetLEDBack");
}

/**
 * @brief Get the current voltage from the Scribbler batteries
 *        Maximum charge from 6 batteries could be up to 6 volts
 *        Manufacturer suggests batteries should be changed below 4.1 volts
 * @return  percentage of battery voltage
*/
double rGetBattery()
{
  char message[1];
  unsigned char receivedMessage[2];

  message[0]= 89; // battery infomation
  // message[1]= (int) message[1] << 8 || (int) message[0] ;  // not used

 
  rSend(message, 1 ,ECHO_COMMAND_OFF, ECHO_SENSOR_OFF , "rGetBattery");
  rReceive (receivedMessage, 2);

  printf ("battery bytes (0/1):  %3u / %3u\n", 
	  (unsigned int) receivedMessage[0],
	  (unsigned int) receivedMessage[1]);
  int battery = ((unsigned int)receivedMessage[0]) << 8 | (unsigned int) receivedMessage[1];
  //printf("%d",battery);
  //Percentage is volts in robot/total voltage
  return (battery/20.9813);
}

/** 
 * Determine if robot has stalled
 *
 * MyroC Reference Manual (http://wiki.roboteducation.org/Myro_Reference_Manual) states
 *    Every time you issue a move command, the stall sensor resets, and it needs to wait
 *    a short time to see whether the motors are stalled.  This means that the sensor 
 *    won't give accurate results if you test it too soon after the robot starts to move.
 *
 * In practice, it may take 0.5-1.0 seconds for rGetStall to sense the robot is stalled
 *
 * @param sampleSize  how many readings are taken for each sensor
 * @pre               sampleSize > 0
 * @return            whether or not robot current has stalled
 * @post              Returns 1 if the robot has stalled
 *                    Returns 0 otherwise.
 */
int rGetStall (int sampleSize)
{
  char message[9];
  unsigned char receivedMessage[2];
  int summedValue = 0;

  message[0]= 79; 
  message[1]= 0; 
  message[2]= 0; 
  message[3]= 0; 
  message[4]= 0;
  message[5]= 0; 
  message[6]= 0;
  message[7]= 0;
  message[8]= 0;

  int i;
  for(i=0; i<sampleSize; i++)
    {
      rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetStall");
      rReceive(receivedMessage, 1);
      summedValue += receivedMessage[0];
    }
     
    return (summedValue > (sampleSize/2));
}