/* * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-movement.c
 * @brief Implementation file for Scribbler 2 movement 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                             rTimedImageDisplay (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>
#include <unistd.h>   // needed for usleep

/*
************************* debugging variable *********************************
*/
int timing_debug = 0;  // 1 :  print messages regarding timing; 0: no debug printing

/*
 ************************ utility motion procedures **************************
 */


/**
 * timer to stop a motion after specified time
 */
void * motion_timer_thread (void * delay_data)
{
  /* make local copy of the delay data, so that subsequent processing does not
   * impact this timer
   */
  motion_timing_data * local_delay_data = ((motion_timing_data *) delay_data);

  if (timing_debug) {
    printf ("starting motion timer for %lf\n", local_delay_data->duration_time);
    printf ("         stored soc_num:  %d\n", local_delay_data->robot_socket_num);
    printf ("         actual soc_num:  %d\n", socket_num);
  }
  
  /* time specified interval of delay */
  long int utime = (long int)(1000000 * local_delay_data->duration_time);
  usleep (utime);

  /* if robot connection still active, and
        robot's motion command not superceded
     then stop robot
   */
 if (timing_debug) {
   printf ("time expired for motion timer for %lf\n", local_delay_data->duration_time);
  }
 
  /* look up sequence number for this robot */
  int i;
  for (i = 0; i < current_num_robots; i++)
     {
       if (robot_sock_array[i].robot_socket_num == socket_num)
          break;
     }
  
  if (i >= current_num_robots)
     {
       printf ("cannot stop motion:  no robot associated with number %d\n",
              (int) socket_num);
       return (void *) 0;
     }

  if (timing_debug) {
    printf (" motion timer found:  local seq number:  %d\n", local_delay_data->motion_seq_num);
    printf ("                     stored seq number:  %d\n", robot_sock_array[i].motion_seq_num);
    printf ("                        stored soc_num:  %d\n", local_delay_data->robot_socket_num);
    printf ("                        actual soc_num:  %d\n", socket_num);
 }

  /* check motion sequence number for this robot */
  if (local_delay_data->motion_seq_num == robot_sock_array[i].motion_seq_num)
    {

      // printf ("stopping motion number %d\n", local_delay_data->motion_seq_num);
      /* store current socket number */
      int temp_socket_num = socket_num;

      /* set socket number for this robot */
      socket_num = robot_sock_array[i].robot_socket_num;

      if (timing_debug) {
        printf ("calling rStop\n");
     } 

      /* ensure only one message processed over Bluetooth at a time */
      pthread_mutex_lock (&bluetooth_lock);
     
       /* stop robot motion */
      char message[9];
      message[0]= (unsigned char) 109;  
      message[1]= (unsigned char) 100;  // full stop value
      message[2]= (unsigned char) 100;
      message[3]= -1;
      message[4]= 0;
      message[5]= 0; 
      message[6]= 0; 
      message[7]= 0; 
      message[8]= 0; 

      /* any past motion timer is no longer relevent */
      robot_sock_array[i].thread_motion_timer_id = 0;
 
      rSend_protected (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rMotors");

      /* timer for this socket number no longer active */
      robot_sock_array[i].thread_motion_timer_id = 0;

      if (timing_debug) {
        printf (" motion timer found:  local seq number:  %d\n", local_delay_data->motion_seq_num);
        printf ("                     stored seq number:  %d\n", robot_sock_array[i].motion_seq_num);
        printf ("                        stored soc_num:  %d\n", local_delay_data->robot_socket_num);
        printf ("                        actual soc_num:  %d\n", socket_num);
     }
      
      /* restore previous socket number */
       socket_num = temp_socket_num;
       
       pthread_mutex_unlock (&bluetooth_lock);

    }
  else
    {
       /* robot motion already superceded by later motion command */
       printf ("motion for robot superceded by later motion\n");
    }

  /* release lock ensuring only one message processed over Bluetooth at a time */

  free (delay_data);
  /* pthread procedure must return void * data */
  return (void *) 0;
}


/* duration-handling procedure for motion commands 
 * once motion is started, motion duration based on parameter
 *
 * options:
 *   duration > 0:   processing blocks for given time (in seconds)
 *                   then processing resumes
 *   duration < 0:   timer thread is established
 *                   processing resumes immediately
 *                   after time expires, 
 *                      if a new motion command has been issued for that robot
 *                         the timer is ignored
 *                      if no new motion command has been issued for that robot
 *                         the motion is stopped
 *   duration = 0:   processing continues with no subsequent timing
 *                   image replaced when new image created for this window
 *
 * win_index identifies index of window to be updated
 */
void process_motion_duration (double duration)
{
  /* if duration > 0, 
   *      block further execution for given time 
   */

  // look up record number for this robot
  int robot_index;  // index of robot in robot_sock_array
  for (robot_index = 0; robot_index < current_num_robots; robot_index++)
    {
      if (robot_sock_array[robot_index].robot_socket_num == socket_num)
	break;
    }
  
  if (robot_index >= current_num_robots)
    {
      printf ("cannot stop motion:  no robot associated with number %d\n",
	      (int) socket_num);
      return;
    }
  
  // printf ("duration processing:  %10.8lf:  ", duration);

  if (duration > 0)
    {
      // printf ("duration > 0\n");

      int utime = (int)(duration * 1000000);
      usleep (utime);
      // send command to stop motion
      
      /* ensure only one message processed over Bluetooth at a time */
      pthread_mutex_lock (&bluetooth_lock);
      
      /* stop robot motion */
      char message[9];
      message[0]= (unsigned char) 109;  
      message[1]= (unsigned char) 100;  // full stop value
      message[2]= (unsigned char) 100;
      message[3]= -1;
      message[4]= 0;
      message[5]= 0; 
      message[6]= 0; 
      message[7]= 0; 
      message[8]= 0; 

      /* any past motion timer is no longer relevent */
      robot_sock_array[robot_index].thread_motion_timer_id = 0;
 
      rSend_protected (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rMotors");

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

  /* if duration < 0, 
   * set timer in separate thread to hide window after designated time
   */
  else if (duration < 0)
    {  
      // printf ("duration < 0\n");
      // create thread, packaging needed data into struct for timer
      motion_timing_data * delay_data = malloc (sizeof (motion_timing_data));
      delay_data->duration_time = -duration;

      /* ensure socket data unchanged during timer setup */
      pthread_mutex_lock (&bluetooth_lock);

      delay_data->robot_socket_num = socket_num;

      /* check sequence number for this motion with this robot */
      delay_data->motion_seq_num = robot_sock_array [robot_index].motion_seq_num;

      /* release lock for finding sequence number */

      pthread_mutex_unlock (&bluetooth_lock);

      /* create timer thread to end motion */
      pthread_t thread;
      int err = pthread_create (&thread, NULL, &motion_timer_thread, 
				delay_data);
      if (err != 0)
	{
	  printf ("\ncannot create timer for non-blocking image;");
	  printf ("error code:  %s\n", strerror(err));
	}
      else 
	{
	  /* new motion timer is active */
	  robot_sock_array[robot_index].thread_motion_timer_id = thread;
	}
    }

  /* duration == 0:  no processing needed */
  else 
    {
      /* any past motion timer is no longer relevent */
      robot_sock_array[robot_index].thread_motion_timer_id = 0;
      // printf ("duration == 0\n");
    }
}


/***********************************************************************/
/* 3. MOVEMENT - MOVEMENT - MOVEMENT - MOVEMENT - MOVEMENT - MOVEMENT  */
/***********************************************************************/

/* Procedures in this section, except rMotors, include a "time" parameter.
 *
 * Throughout, "time" is interpreted as follows:
 *     if time > 0, the call to the procedure is "blocking:
 *            That is, a call to the procedure starts a motion, and
 *            all other processing stops for the specified time (in seconds).
 *            After the specified time interval, the motion stops,
 *            and processing in the main program continues.
 *     If time == 0, the procedure is "non-blocking":
 *            That is, the procedure starts a motion, 
 *            and that motion continues until another motion command is issued.
 *            Once the motion starts, other processing in the main
 *            program continues.
 *     If time < 0, the procedure is "non-blocking": 
 *            That is, a call to the procedure starts a motion,
 *            and a timer is started for the abs("time").
 *            Once the time interval is completed,
 *               if no other motion command has been issued, 
 *                  a command to stop robot motion is given
 *               if another motion command has been issued (before the
 *                  specified time), the new command is executed immediately,
 *                  and the timer is ignored.
 *           For example, suppose rForward is called with a time of -5 seconds:
 *               subsequent processing in the main program continues
 *               if numerous beeps are issued or the camera is used or
 *                  a series of non-motion commands are issued, then
 *                  the rForward action stops in 5 seconds.
 *               if an rRightTurn command is issued after 2 seconds,
 *                  then the rForward command is overridden by rRightTurn
 *                  and the timer for rForward (after another 3 seconds)
 *                  is ignored.
 *
 * Program termination with timed, non-blocking commands:
 *    When a non-blocking time is specified (i.e., time < 0), processing within 
 *    the controlling program continues.  However, the robot's Bluetooth connection
 *    (established with rConnect) is not severed until the non-blocking timer has
 *    expired and the motion stopped.  In particular, rDisconnect blocks until
 *    the specified non-blocking time is completed and the program tells the
 *    robot to stop.
 */

/**
 * turn Scribbler left for a specified time and speed
 * @param speed  the rate at which the robot should move left
 *               linear range:  -1.0 specifies right turn at full speed
 *                              0.0 specifies no turn
 *                              1.0 specifies left turn at full speed
 * @param time   specifies the duration of the turn
 *               if negative:    robot continues to turn until given another
 *                               motion command or disconnected (non-blocking)
 *               if nonnegative: robot turns for the given duration, in seconds
 */

void rTurnLeft (double speed, double time)
 { 
    rMotors(speed,-speed);
    process_motion_duration (time);
}

/**
 * turn Scribbler right for a specified time and speed  
 *  @param speed  the rate at which the robot should move right
 *                linear range: -1.0 specifies left turn at full speed
 *                              0.0 specifies no turn
 *                              1.0 specifies right turn at full speed
 *  @param time   specifies the duration of the turn
 *                if negative:    robot continues to turn until given another
 *                                motion command or disconnected (non-blocking)
 *                if nonnegative: robot turns for the given duration, in seconds
 */
void rTurnRight (double speed, double time)
{
    rMotors(-speed,speed);
    process_motion_duration (time);
}

/** 
 * turn Scribbler in direction for a specified time and speed
 * @param direction  direction of turn, based on looking from
 *                   the center of the robot and facing forward
 * @param speed      the rate at which the robot should move forward
 *                   linear range:  -1.0 specifies turn at full speed
 *                                  0.0 specifies no turn
 *                                  1.0 specifies turn at full speed
 * @param time       specifies the duration of the turn
 *                   if negative: robot continues to turn until given another
 *                                motion command or disconnected
 *                   if nonnegative: robot turns for the given duration, in seconds
 * @pre              direction is "left" or "right", case insensitive
 */
void rTurnSpeed (char * direction, double speed, double time)
{
  int len = strlen (direction)+1;
  char workingString [len];
  int i;
  // Convert direction to lower case
  for (i = 0; i < len; i++)
    workingString[i] = tolower (direction[i]);

  if (strcmp(workingString, "left") == 0)
    {
      rTurnLeft(speed, time);                                          
    }
  else if (strcmp(workingString, "right") == 0)
    {
      rTurnRight(speed, time);                                          
    }
  else
    printf("error: rTurnSpeed takes either left or right. Received parameter: %s \n", direction);
}

/**
 * moves Scribbler forward for a specified time and speed
 * @param speed   the rate at which the robot should move forward
 *                linear range:  -1.0 specifies move backward at full speed
 *                               0.0 specifies no forward/backward movement
 *                               1.0 specifies move forward at full speed
 * @param time    specifies the duration of movement
 *                if negative:    robot continues to move forward until given another
 *                                motion command or disconnected (non-blocking)
 *                if nonnegative: robot moves forward for the given duration, in
 *                                seconds
 */
void rForward (double speed, double time)
{
    rMotors(speed,speed);
    process_motion_duration (time);
}

/** 
 * moves Scribbler forward at the largest possible speed for a specified time 
 * @param time  specifies the duration of movement
 *              if negative: robot continues to move forward until given another
 *                           motion command or disconnected (non-blocking)
 *              if nonnegative: robot moves forward for the given duration, in
 *                              seconds
 * @warning     may take longer than usual to execute
 */
void rFastForward (double time)
{
  char * forwardness = rGetForwardness();
  if (forwardness[0] == 'f')
    {
      rForward(1.0, time);
    }// if was fluke-forward
  else
    {
      char message[9];
      message[0]= 109;  
      message[1]= (unsigned char) 255;
      message[2]= (unsigned char) 255;
      message[3]= -1;
      message[4]= 0;
      message[5]= 0; 
      message[6]= 0; 
      message[7]= 0; 
      message[8]= 0; 
      rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rFastForward");

      process_motion_duration (time);

      rSetForwardness(forwardness);
       
    } //else scribbler-forward
}

/**
 * moves Scribbler backward for a specified time and speed  
 * @param speed  the rate at which the robot should move backward
 *               linear range:  -1.0 specifies move forward at full speed
 *                              0.0 specifies no forward/backward movement
 *                              1.0 specifies move backward at full speed
 * @param time   specifies the duration of the turn
 *               if negative: robot continues to go backward until given another
 *                            motion command or disconnected (non-blocking)
 *               if nonnegative: robot moves backward for the given duration, in
 *                               seconds
 *                  
 */
void rBackward (double speed, double time)
{
    rMotors(-speed,-speed);
    process_motion_duration (time);
}



/**
 * move robot with given speeds for the left and right motors
 * continues until given another motion command or disconnected (non-blocking)
 * @param leftSpeed  the rate at which the left wheel should turn
 *              linear range:  -1.0 specifies move backward at full speed
 *                              0.0 specifies no forward/backward movement
 *                              1.0 specifies move forwardward at full speed
 * @param rightSpeed  the rate at which the right wheel should turn
 *              linear range:  -1.0 specifies move backward at full speed
 *                              0.0 specifies no forward/backward movement
 *                              1.0 specifies move forward at full speed
 */
void rMotors (double leftSpeed, double rightSpeed)
{
  /* Ensure correct range - from -1 to 1 */
  leftSpeed = MIN(leftSpeed, 1.0);
  leftSpeed = MAX(leftSpeed, -1.0);
  rightSpeed = MIN(rightSpeed, 1.0);
  rightSpeed = MAX(rightSpeed, -1.0);

  int lP = (int)((leftSpeed + 1.0) * 100);
  int rP = (int)((rightSpeed + 1.0) * 100);

  /* prepare message to Scribbler */
  char message[9];

  message[0]= 109;  
  message[1]= (unsigned char) lP;
  message[2]= (unsigned char) rP;
  message[3]= -1;
  message[4]= 0;
  message[5]= 0; 
  message[6]= 0; 
  message[7]= 0; 
  message[8]= 0; 
 
  rSend (message, 9, ECHO_COMMAND_ON, ECHO_SENSOR_ON, "rMotors");

  /* log that another motion command has been issued for this robot */
  /* ensure only one message processed over Bluetooth at a time */
  pthread_mutex_lock (&bluetooth_lock);

  /* look up sequence number for this robot */
  int i;
  for (i = 0; i < current_num_robots; i++)
     {
       if (robot_sock_array[i].robot_socket_num == socket_num)
          break;
     }
  
  if (i >= current_num_robots)
     {
       printf ("cannot stop motion:  no robot associated with number %d\n",
               (int) socket_num);
       /* release lock ensuring only one message processed over Bluetooth at a time */
       pthread_mutex_unlock (&bluetooth_lock);
       return ;
     }

  /* check motion sequence number for this robot */
  robot_sock_array[i].motion_seq_num++;

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

}

/**
 * directs robot to stop movement
 */
void rStop()
{
  rMotors(0.0,0.0);
}

/**
 * cuts power to the motor of the robot
 */
void rHardStop()
{

  /* send hard stop command to robot */
  char message[9];
  message[0]= 108;  
  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, "rHardStop");
}

