/* * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-sensor.c 
 * @brief Implementation file for the MyroC sensor commands
 *
 * 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 (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           

    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>

/**************************************************************************/
/* 2. SENSORS - SENSORS - SENSORS - SENSORS - SENSORS - SENSORS - SENSORS */
/**************************************************************************/

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*  Group a:  Scribbler Sensors                                          */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
 * @brief Get the average values of each of the three light sensors in an array.
 * Values of each light sensor can somewhat (typically under 5%-10%).
 * To even out variability, the sensor can be queried sampleSize times
 * and an average obtained.
 * @param lightSensors  array to store intensity values
 * @param sampleSize   how many readings are taken for each sensor
 * @pre                space already allocated for lightSensors array
 *                     sampleSize > 0
 * @post  
 *                     lightSensors[0] gives average value for left sensor
 *                     lightSensors[1] gives average value for middle sensor
 *                     lightSensors[2] gives average value for right sensor
 *                     Intensity values near 0 represent bright light
 *                     Intensities may extend to about 65000 for a very dark region.
*/
void rGetLightsAll (int lightSensors[3], int sampleSize)
{
  char message[9];
  unsigned char receivedMessage[6];

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

  int i;
  for(i = 0; i < sampleSize; i++){
    rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetLightsAll");
    rReceive(receivedMessage, 6);
    /* sensor values returned as high-byte/low-byte pairs 
       combine bytes with bitwise-or */
    lightSensors[0] += (int)receivedMessage[0] << 8 | (int) receivedMessage[1];
    lightSensors[1] += (int)receivedMessage[2] << 8 | (int) receivedMessage[3];
    lightSensors[2] += (int)receivedMessage[4] << 8 | (int) receivedMessage[5];
  }

  lightSensors[0] /= sampleSize;
  lightSensors[1] /= sampleSize;
  lightSensors[2] /= sampleSize;
}

/**
 * @brief Get the average values of a specified light sensor.
 * Values of each light sensor can vary somewhat (typically under 5%-10%).
 * To even out variability, the sensor can be queried sampleSize times
 * and an average obtained.
 * @param sensorName   name of the light sensor
 * @pre                sensorName is "left", "center", "middle", or "right"
 *                     (not case sensitive)
 *                     designations "center" and "middle" are alternatives
 *                     for the same light sensor
 * @param sampleSize   how many readings are taken for the sensor
 * @pre                sampleSize > 0
 * @return             reading from the specified light sensor, averaged
 *                     over sampleSize number of data samples
 *                     if sensorName invalid, returns -1.0
 */
int rGetLightTxt (const char * sensorName, int sampleSize)
{
  int len = strlen(sensorName) + 1;
  char workingString [len];
  int i;
  for (i = 0; i < len; i++)
    workingString[i] = tolower (sensorName[i]);

  char message[9];
  unsigned char receivedMessage[2];
  int summedValue = 0;

  if(!strcmp(workingString, "left")){
    message[0]= 67; 
  }
  else if ((!strcmp(workingString, "center"))
            || (!strcmp(workingString, "middle"))){
    message[0]= 68;
  }
  else if (!strcmp(workingString, "right")){
    message[0]= 69;
  }
  else return -1.0;

  message[1] = 0; 
  message[2] = 0; 
  message[3] = 0; 
  message[4] = 0;
  message[5] = 0; 
  message[6] = 0;
  message[7] = 0;  
  message[8] = 0;  
 
  /*  query sensor specified number of times and average */
  for(i = 0; i < sampleSize; i++){
    rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetLightTxt");
    rReceive(receivedMessage, 2);
    /* sensor values returned as high-byte/low-byte pairs 
       combine bytes with bitwise-or */
    summedValue += (int)receivedMessage[0] << 8 | (int) receivedMessage[1];
  }

  return summedValue / sampleSize;
}

/**
 * @brief Get an array of true/false values regarding the presence of an obstacle,
 * based on the average values of each of the three IR sensors.
 * Since readings of each light sensor can vary substantially,
 *    each sensor can be queried sampleSize times and an average obtained.
 * @param irSensors   array to store intensity values
 * @param sampleSize  how many readings are taken for each sensor
 * @pre               space already allocated for irSensors array
 *                    sampleSize > 0
 * @post  
 *                    irSensors[0] checks obstacle for left sensor
 *                    irSensors[1] checks obstacle for right sensor
 * @post              for each irSensors array value
 *                    return 0 indicates no obstacle detected
 *                    return 1 indicates obstacle detected  
*/
void rGetIRAll (int irSensors[2], int sampleSize)
{
     char message[9]; 
     unsigned char receivedMessage[2];
     int summedValues[2];

     message[0] = 73; 
     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
     summedValues[0] = 0;
     summedValues[1] = 0;

     int i;
     for(i = 0; i < sampleSize; i++){
       rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetIRAll");
       rReceive(receivedMessage, 2);
       summedValues[0] += receivedMessage[0];
       summedValues[1] += receivedMessage[1];
     }

     /* obstacle sensor is misnamed, returning 1 if no obstacle and 0 if obstacle
        the following code switches the output, so 1 means obstacle exists
     */
     /* obstacle present if obstacle found in at least half the queries */
     irSensors[0] = (summedValues[0] < (sampleSize/2));
     irSensors[1] = (summedValues[1] < (sampleSize/2));
}

/**
 * @brief Use specified IR sensor to determine if obstacle is present.
 * Since values of each light sensor can vary substantially,
 * the sensor can be queried sampleSize times and an average obtained.
 * @param sensorName   name of the light sensor
 * @pre                sensorName is "left" or "right"
 *                     (not case sensitive)
 * @param sampleSize   how many readings are taken for the sensor
 * @pre                sampleSize > 0
 * @return             true/false (0/1) determination of obstacle, based on IR
 *                     sensorName sensor, averaged over sampleSize number of
 *                     data samples
 * @post               return 0 indicates no obstacle detected
 *                     return 1 indicates obstacle detected
*/
int rGetIRTxt (const char * sensorName, int sampleSize)
{
  int len = strlen(sensorName) + 1;
  char workingString [len];
  int i;
  for (i = 0; i < len; i++)
    workingString[i] = tolower (sensorName[i]);

  char message[9];
  unsigned char receivedMessage[1];
  int summedValue = 0;;
     
  if(strcmp(workingString, "left") == 0){
    message[0]= 71;
  }
  else if (strcmp(workingString, "right") == 0){
    message[0]= 72; 
  }
  else
    {
      return -1;
    }

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

  /* query sensors needed number of times to get average */
  for(i = 0; i < sampleSize; i++){
    rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetIRTxt");
    rReceive(receivedMessage, 1);
    summedValue += receivedMessage[0];
  }

     /* obstacle sensor is misnamed, returning 1 if no obstacle and 0 if obstacle
        the following code switches the output, so 1 means obstacle exists
     */
     /* obstacle present if obstacle found in at least half the queries */
  return (summedValue <= (sampleSize/2));
}
  
/**
 * @brief Use Scribbler 2 line sensors of Scribbler to check for a dark line
 * on a light surface under the robot.
 * Since values of each light sensor can vary substantially,
 *    the sensor can be queried sampleSize times and an average obtained.
 * @warning            results of these sensors may be flakey!
 * @param lineSensors  array to store line values detected
 * @param sampleSize   how many readings are taken for each sensor
 * @pre                space already allocated for lineSensors array
 *                     sampleSize > 0
 * @post               lineSensors[0] checks left sensor for line
 *                     lineSensors[1] checks right sensor for line
 *                      (left/right based on Scribbler-forward orientation)
 * @post               for each line sensors array value
 *                     return 0 indicates line  is identified (light color under Scribbler)
 *                     return 1 indicates line is not identified (dark color)
 */
void rGetLine (int lineSensors[2], int sampleSize)
{
  char message[9];
  unsigned char receivedMessage[2];
  int summedValues[2];

  message[0] = 76; 
  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
  summedValues[0] = 0;
  summedValues[1] = 0;

  int i;
  for(i=0; i<sampleSize; i++){
    rSend(message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_OFF, "rGetLine");
    rReceive(receivedMessage, 2);
    summedValues[0] += receivedMessage[0];
    summedValues[1] += receivedMessage[1];
  }

  /* line present if detected in fewer than half the queries */
  lineSensors[0] = (summedValues[0] >= (sampleSize/2));
  lineSensors[1] = (summedValues[1] >= (sampleSize/2));
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*  Group b:  Fluke Sensors                                              */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */


/**
 * @brief Set the amount of power for the dongle's IR sensors
 * @param power  the desired power level for the IR sensors
 * @pre          power is between 0 and 255 (inclusive)
*/
void rSetIRPower (int power)
{
  if (power > 255)
    power = 255;
  if (power < 0)
    power = 0;
  char message[2]; 
  message[0] = 120;
  message[1] = power;
  
  rSend(message, 2, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rSetIRPower");   
}

/**
 * @brief Get the average values of the three obstacle sensors in an array.
 * Since readings of each obstacle sensor can vary substantially (successive
 *      readings may differ by several hundred or more),
 *      each sensor can be queried sampleSize times and an average obtained.
 * @param obstSensors   array to store intensity values
 * @param sampleSize    how many readings are taken for each sensor
 * @pre                 space already allocated for obstSensors array
 *                      sampleSize > 0
 * @post                obstSensors[0] gives average value for left sensor
 *                      obstSensors[1] gives average value for middle sensor
 *                      obstSensors[2] gives average value for right sensor
 *                      Returned values are between 0 and 6400
 *                      Obstacle values near 0 represent no obstacle seen
 *                      Obstacle values may approach 6400 as obstacle gets close.
 * @warning             As battery degrades, sensor readings degrade,
 *                      yielding systematically lower numbers.
 */
void rGetObstacleAll (int obstSensors[3], int sampleSize)
{
  obstSensors[0] = rGetObstacleTxt("left", sampleSize);
  obstSensors[1] = rGetObstacleTxt("middle", sampleSize);
  obstSensors[2] = rGetObstacleTxt("right", sampleSize);
}

/** 
 * @brief Get the average values of a specified obstacle (IR) sensor.
 * Since values of each obstacle sensor can vary substantially (successive
 *      readings may differ by several hundred or more),
 *      the sensor can be queried sampleSize times and an average obtained.
 * @param sensorName  name of the obstacle sensor
 * @pre               sensorName is "left", "center", "middle", or "right"
 *                    (not case sensitive)
 *                    designations "center" and "middle" are alternatives
 *                    for the same light sensor
 * @param sampleSize  how many readings are taken for the sensor
 * @pre               space already allocated for vals array
 *                    sampleSize > 0
 * @return            reading from the specified obstacle sensor, averaged
 *                       over sampleSize number of data samples
 *                    Returned values are between 0 and 6400
 *                    Obstacle values near 0 represent no obstacle seen
 *                    Obstacle values may approach 6400 as obstacle gets close.
 * @warning           As battery degrades, sensor values degrade,
 *                    yielding systematically lower numbers.
 */
int rGetObstacleTxt (const char * sensorName, int sampleSize)
{
  int len = strlen(sensorName) + 1;
  char workingString [len];
  int i;
  for (i = 0; i < len; i++)
    workingString[i] = tolower (sensorName[i]);

  char message[1];
  unsigned char result[2];
  int summedValue = 0;

  if (strcmp("left", workingString) == 0)
    {
      message[0] = 85;
    }
  else if ((strcmp("middle", workingString) == 0) 
      || (strcmp("center", workingString) == 0))
    {
      message[0] = 86;
    }
  else if (strcmp("right", workingString) == 0)
    {
      message[0] = 87;
    }
  else 
    return -1;

  for(i = 0; i < sampleSize; i++){
    rSend (message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rGetObstacleTxt");
    rReceive (result, 2);
    summedValue += ((int)(result[0])) << 8 | ((int) result[1]);
  }

    return summedValue / sampleSize;
}

/**
 * @brief Read the Fluke's virtual light sensors.
 * Each sensor reports a total intensity in the left, middle, or right of the
 *    Fluke's camera
 *  Since readings of each brightness sensor can vary substantially (successive
 *     readings may differ by 5000-10000),
 *     each sensor can be queried sampleSize times and an average obtained.
 * @param brightSensors  array to store intensity values
 * @param sampleSize     how many readings are taken for each sensor
 * @pre                  space already allocated for brightSensors array
 *                       sampleSize > 0
 * @post                 brightSensors[0] gives average value for left sensor
 *                       brightSensors[1] gives average value for middle sensor
 *                       brightSensors[2] gives average value for right sensor
 *                       Brightness values near 0 represent bright light
 *                       Brightness values may extend to about 65535 for a very dark region.
 */
void rGetBrightAll (int brightSensors[3], int sampleSize)
{
  brightSensors[0] = rGetBrightTxt("left", sampleSize);
  brightSensors[1] = rGetBrightTxt("center", sampleSize);
  brightSensors[2] = rGetBrightTxt("right", sampleSize);
}

/**
 * @brief Reads one of the Fluke's virtual light sensors.
 * Each sensor reports a total intensity in the left, middle, or right of the
 *    Fluke's camera
 * Since values of each obstacle sensor can vary substantially (successive
 *    readings may differ by 5000-10000),
 *    the sensor can be queried sampleSize times and an average obtained.
 * @param sensorName  name of the bright sensor
 * @pre               sensorName is "left", "center", "middle", or "right"
 *                    (not case sensitive)
 *                    designations "center" and "middle" are alternatives
 *                    for the same bright sensor
 * @param sampleSize  how many readings are taken for the sensor
 * @pre               sampleSize > 0
 * @return            reading from the specified bright sensor, averaged
 *                    over sampleSize number of data samples
 *                    Brightness values near 0 represent bright light
 *                    Brightness values may extend to about 65535 for a very dark region.
 */
int rGetBrightTxt (char * sensorName, int sampleSize)
{
  int len = strlen(sensorName) + 1;
  char workingString [len];
  int i;
  for (i = 0; i < len; i++)
    workingString[i] = tolower (sensorName[i]);

  char message[1];
  unsigned char returned[2];
  int summedValue = 0;
     
  if (strcmp(workingString, "left") == 0){
    message[0] = 85;
  }
  else if (strcmp(workingString, "center") == 0){
    message[0] = 86;
  }
  else if (strcmp(workingString, "middle") == 0){
    message[0] = 86;
  }
  else if (strcmp(workingString, "right") == 0){
    message[0] = 87;
  }
  else return -1;

  for(i=0; i<sampleSize; i++){
    // C++ sends 3 bytes, but hangs
    rSend(message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rGetBrightTxt");
  rReceive(returned, 2);
  summedValue += ( (int)returned[1]<<8 | (int)returned[2] );
  }

  return (int) ((double)summedValue / (double) sampleSize);
}

/**
 * returns information about the robot's dongle, firmware, and 
 * communication mode as a 60 character array in infoBuffer. 
 * @param infoBuffer  a pre-defined, 60-character array 
 * @post              infoBuffer contains relevant robot information
 */
 void rGetInfo (char * infoBuffer)
 {
   char message[1];
     
   message[0]= 80;
   rSend(message, 1, ECHO_COMMAND_OFF, ECHO_SENSOR_OFF, "rGetInfo");
   rReceive((unsigned char *) infoBuffer, 60);
 }
