/* * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * @file  MyroC-camera.c    
 * @brief Implementation file for downloading an image
 *        from the Scribbler 2 camera to the local workstation
 *
 * 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 (prototypes in MyroC-utilities.h
                       implemented in MyroC-utilities.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
    Beep2                        
    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

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

/* 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 <fcntl.h>       // needed for open
#include <unistd.h>      // needed for close, read, write
//#include <termios.h>     // needed to set Bluetooth communication parameters


/***********************************************************************/
/* 4.) PICTURES PICTURES PICTURES PICTURES PICTURES PICTURES PICTURES  */
/***********************************************************************/
/**
 * This section contains functions for taking and manipulating images
 * All images are constrained with height <= 266 and width <= 427
 *
 * images from robot cameras have varying sizes, depending on the Fluke
 *     images for the original Fluke are 192 (height) by 256 (width)
 *     low-resolution images for the Fluke 2 are 266 by 427
 *     high-resolution images for the fluke 2 are 800 by 1280
 *
 * Bluetooth communication constrains the time required for the Fluke
 *        to take a picture
 *     Typical times:   original fluke:      2- 4 seconds
 *                      Fluke 2 (low res):   4- 6 seconds
 *                      Fluke 2 (high res): 25-30 seconds
 *
 *     BASED ON TIMINGS AND MEMORY CONSIDERATIONS, Myro C ALLOWS
 *     ONLY LOW RESOLUTION IMAGES
 *
 * the Picture struct allows direct access to Pixel data
 * Pictures can be saved and loaded as .jpeg files
 *
 * @note Following standard mathematical convention for a 2D matrix, 
 *       all references to a pixel are given within an array as [row][col]
 * @note    user-defined images may have any size, as long as
 *          height <= 266 and width <= 427
 * @note Following standard mathematical convention for a 2D matrix, 
 *       all references to a pixel are given within an array as [row][col]
 *
 * @warning The Picture struct is defined to be sufficiently large
 *          to store several low-resolution camera images (340756 bytes each)
 *             Experimentally, an array of up to 94 (not 95) Pictures is allowed
 *             However, the display of images requires that image data
 *                be copied, so display of many images may not work
 *          If a program hangs when working with Picture variables,
 *             the issue may involve lack of space on the runtime stack.
 *             To utilize a modest number of Pictures,   
 *             use "ulimit -s"  command, as needed, in a terminal window
 *             For example, ulimit -s 32768
 *             Sizes above 32768 may not be allowed in Linux or Mac OS X
 */

/**
 * Use the camera to take a photo
 * @return Picture
 */
Picture rTakePicture()
{
  Picture result;

  /* camera data stord in struct for this robot within robot_soc_array */
  if (current_num_robots == 0)
    {
      result.height = result.width = 0;
      printf ("no connection to robot; cannot take picture\n");
      return result;
    }

  /* 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)
    {
      result.height = result.width = 0;
      printf ("Fluke type is unknown; cannot take picture\n");
      return result;
    }

  // determine Fluke type, if not yet known 
    if (strcmp (robot_sock_array [robot_index].fluke_type, "unknown") == 0)
    {
      printf ("checking hardware in rTakePicture\n");
      rCheckHardwareVersionSetCameraSize ('n');
    }

  int height = robot_sock_array [robot_index].camera_height;
  int width = robot_sock_array [robot_index].camera_width;
  int size = width*height;
  result.width = width;	
  result.height = height;
  Pixel pix;
  
  //printf ("Fluke camera set to %i (height) by %i (width)\n",
  //        robot_sock_array [robot_index].camera_height,
  //        robot_sock_array [robot_index].camera_width);

  unsigned char * yuv_buffer 
    = (unsigned char*)malloc(sizeof(unsigned char) * size);
  
  char message[1];
  message[0]=83; //83 is the command to take a picture;
  rSend(message,1,ECHO_COMMAND_OFF,ECHO_SENSOR_OFF, "rTakePicture"); //no echos

  rReceive(yuv_buffer,size); //receive the YUV data into the yuv_buffer
  
  /*most of the below code comes from Scibbler.CPP and calico (Python)
    Changes relate to C syntax and putting result into Picture struct */
  
  int vy, vu, y1v, y1u, uy, uv, y2u, y2v;
  
  int Y = 0,U = 0,V = 0;
  
  int i,j;
  
  for(i = 0; i < height; i++) {
    for(j = 0; j < width; j++) {
      if( j >= 3 ) {
	vy = -1; vu = -2; y1v = -1; y1u = -3; uy = -1; uv = -2;
	y2u = -1; y2v = -3;
      }
      else {
	vy = 1; vu = 2; y1v = 3; y1u = 1; uy = 1; uv = 2; 
	y2u = 3; y2v = 1;
      }
      
      //VYUY VYUY VYUY
      if( (i * width + j) % 4 == 0 ) {
	V = (int)yuv_buffer[i * width + j];
	Y = (int)yuv_buffer[i * width + j + vy];
	U = (int)yuv_buffer[i * width + j + vu];
      }
      if( (i * width + j) % 4 == 1 ) {
	Y = (int)yuv_buffer[i * width + j];
	V = (int)yuv_buffer[i * width + j + y1v];
	U = (int)yuv_buffer[i * width + j + y1u];
      }
      if( (i * width + j) % 4 == 2 ) {
	U = (int)yuv_buffer[i * width + j];
	Y = (int)yuv_buffer[i * width + j + uy];
	V = (int)yuv_buffer[i * width + j + uv];
      }
      if( (i * width + j) % 4 == 3 ) {               
	Y = (int)yuv_buffer[i * width + j];
	U = (int)yuv_buffer[i * width + j + y2u];
	V = (int)yuv_buffer[i * width + j + y2v];
      }
      
      U = U - 128;
      V = V - 128;
    
      pix.R=(unsigned char) MAX(MIN(Y + 1.13983 * V, 255.0), 0.0);
      // from MyroCpp
      pix.G=(unsigned char) MAX(MIN(Y - 0.39466 * U - 0.7169 * V, 255.0),0.0);
      // from calico/Python
      pix.G=(unsigned char) MAX(MIN(Y - 0.39466 * U - 0.58060 * V, 255.0),0.0);
      pix.B=(unsigned char) MAX(MIN(Y + 2.03211 * U, 255.0), 0.0);
      result.pix_array[i][j]=pix; //put the pixel into the pix array. 
  
    }
  }
  
  free(yuv_buffer); //no longer need yuv_buffer
  return result;
}


