text: /*
  Copyright (c) 2018, Raspberry Pi (Trading) Ltd.
  Copyright (c) 2013, Broadcom Europe Ltd.
  Copyright (c) 2013, James Hughes
  All rights reserved.
  
  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
      * Redistributions of source code must retain the above copyright
        notice, this list of conditions and the following disclaimer.
      * Redistributions in binary form must reproduce the above copyright
        notice, this list of conditions and the following disclaimer in the
        documentation and/or other materials provided with the distribution.
      * Neither the name of the copyright holder nor the
        names of its contributors may be used to endorse or promote products
        derived from this software without specific prior written permission.
  
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
  
  /**
   * \file RaspiStill.c
   * Command line program to capture a still frame and encode it to file.
   * Also optionally display a preview/viewfinder of current camera input.
   *
   * Description
   *
   * 3 components are created; camera, preview and JPG encoder.
   * Camera component has three ports, preview, video and stills.
   * This program connects preview and stills to the preview and jpg
   * encoder. Using mmal we don't need to worry about buffers between these
   * components, but we do need to handle buffers from the encoder, which
   * are simply written straight to the file in the requisite buffer callback.
   *
   * We use the RaspiCamControl code to handle the specific camera settings.
   */
  
  // We use some GNU extensions (asprintf, basename)
  #ifndef _GNU_SOURCE
  #define _GNU_SOURCE
  #endif
  
  #include <stdio.h>
  #include <stdlib.h>
  #include <ctype.h>
  #include <string.h>
  #include <memory.h>
  #include <unistd.h>
  #include <errno.h>
  #include <sysexits.h>
  
  #include "bcm_host.h"
  #include "interface/vcos/vcos.h"
  
  #include "interface/mmal/mmal.h"
  #include "interface/mmal/mmal_logging.h"
  #include "interface/mmal/mmal_buffer.h"
  #include "interface/mmal/util/mmal_util.h"
  #include "interface/mmal/util/mmal_util_params.h"
  #include "interface/mmal/util/mmal_default_components.h"
  #include "interface/mmal/util/mmal_connection.h"
  #include "interface/mmal/mmal_parameters_camera.h"
  
  #include "RaspiCommonSettings.h"
  #include "RaspiCamControl.h"
  #include "RaspiPreview.h"
  #include "RaspiCLI.h"
  #include "RaspiTex.h"
  #include "RaspiHelpers.h"
  
  // TODO
  //#include "libgps_loader.h"
  
  #include "RaspiGPS.h"
  
  #include <semaphore.h>
  #include <math.h>
  #include <pthread.h>
  #include <time.h>
  
  // Standard port setting for the camera component
  #define MMAL_CAMERA_PREVIEW_PORT 0
  #define MMAL_CAMERA_VIDEO_PORT 1
  #define MMAL_CAMERA_CAPTURE_PORT 2
  
  // Stills format information
  // 0 implies variable
  #define STILLS_FRAME_RATE_NUM 0
  #define STILLS_FRAME_RATE_DEN 1
  
  /// Video render needs at least 2 buffers.
  #define VIDEO_OUTPUT_BUFFERS_NUM 3
  
  #define MAX_USER_EXIF_TAGS      32
  #define MAX_EXIF_PAYLOAD_LENGTH 128
  
  /// Frame advance method
  enum
  {
     FRAME_NEXT_SINGLE,
     FRAME_NEXT_TIMELAPSE,
     FRAME_NEXT_KEYPRESS,
     FRAME_NEXT_FOREVER,
     FRAME_NEXT_GPIO,
     FRAME_NEXT_SIGNAL,
     FRAME_NEXT_IMMEDIATELY
  };
  
  /// Amount of time before first image taken to allow settling of
  /// exposure etc. in milliseconds.
  #define CAMERA_SETTLE_TIME       1000
  
  /** Structure containing all state information for the current run
   */
  typedef struct
  {
     RASPICOMMONSETTINGS_PARAMETERS common_settings;     /// Common settings
     int timeout;                        /// Time taken before frame is grabbed and app then shuts down. Units are milliseconds
     int quality;                        /// JPEG quality setting (1-100)
     int wantRAW;                        /// Flag for whether the JPEG metadata also contains the RAW bayer image
     char *linkname;                     /// filename of output file
     int frameStart;                     /// First number of frame output counter
     MMAL_PARAM_THUMBNAIL_CONFIG_T thumbnailConfig;
     int demoMode;                       /// Run app in demo mode
     int demoInterval;                   /// Interval between camera settings changes
     MMAL_FOURCC_T encoding;             /// Encoding to use for the output file.
     const char *exifTags[MAX_USER_EXIF_TAGS]; /// Array of pointers to tags supplied from the command line
     int numExifTags;                    /// Number of supplied tags
     int enableExifTags;                 /// Enable/Disable EXIF tags in output
     int timelapse;                      /// Delay between each picture in timelapse mode. If 0, disable timelapse
     int fullResPreview;                 /// If set, the camera preview port runs at capture resolution. Reduces fps.
     int frameNextMethod;                /// Which method to use to advance to next frame
     int useGL;                          /// Render preview using OpenGL
     int glCapture;                      /// Save the GL frame-buffer instead of camera output
     int burstCaptureMode;               /// Enable burst mode
     int datetime;                       /// Use DateTime instead of frame#
     int timestamp;                      /// Use timestamp instead of frame#
     int restart_interval;               /// JPEG restart interval. 0 for none.
  
     RASPIPREVIEW_PARAMETERS preview_parameters;    /// Preview setup parameters
     RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters
  
     MMAL_COMPONENT_T *camera_component;    /// Pointer to the camera component
     MMAL_COMPONENT_T *encoder_component;   /// Pointer to the encoder component
     MMAL_COMPONENT_T *null_sink_component; /// Pointer to the null sink component
     MMAL_CONNECTION_T *preview_connection; /// Pointer to the connection from camera to preview
     MMAL_CONNECTION_T *encoder_connection; /// Pointer to the connection from camera to encoder
  
     MMAL_POOL_T *encoder_pool; /// Pointer to the pool of buffers used by encoder output port
  
     RASPITEX_STATE raspitex_state; /// GL renderer state and parameters
  
  } RASPISTILL_STATE;
  
  /** Struct used to pass information in encoder port userdata to callback
   */
  typedef struct
  {
     FILE *file_handle;                   /// File handle to write buffer data to.
     VCOS_SEMAPHORE_T complete_semaphore; /// semaphore which is posted when we reach end of frame (indicates end of capture or fault)
     RASPISTILL_STATE *pstate;            /// pointer to our state in case required in callback
  } PORT_USERDATA;
  
  static void store_exif_tag(RASPISTILL_STATE *state, const char *exif_tag);
  
  /// Command ID's and Structure defining our command line options
  enum
  {
     CommandQuality,
     CommandRaw,
     CommandTimeout,
     CommandThumbnail,
     CommandDemoMode,
     CommandEncoding,
     CommandExifTag,
     CommandTimelapse,
     CommandFullResPreview,
     CommandLink,
     CommandKeypress,
     CommandSignal,
     CommandGL,
     CommandGLCapture,
     CommandBurstMode,
     CommandDateTime,
     CommandTimeStamp,
     CommandFrameStart,
     CommandRestartInterval,
  };
  
  static COMMAND_LIST cmdline_commands[] =
  {
     { CommandQuality, "-quality",    "q",  "Set jpeg quality <0 to 100>", 1 },
     { CommandRaw,     "-raw",        "r",  "Add raw bayer data to jpeg metadata", 0 },
     { CommandLink,    "-latest",     "l",  "Link latest complete image to filename <filename>", 1},
     { CommandTimeout, "-timeout",    "t",  "Time (in ms) before takes picture and shuts down (if not specified, set to 5s)", 1 },
     { CommandThumbnail,"-thumb",     "th", "Set thumbnail parameters (x:y:quality) or none", 1},
     { CommandDemoMode,"-demo",       "d",  "Run a demo mode (cycle through range of camera options, no capture)", 0},
     { CommandEncoding,"-encoding",   "e",  "Encoding to use for output file (jpg, bmp, gif, png)", 1},
     { CommandExifTag, "-exif",       "x",  "EXIF tag to apply to captures (format as 'key=value') or none", 1},
     { CommandTimelapse,"-timelapse", "tl", "Timelapse mode. Takes a picture every <t>ms. %d == frame number (Try: -o img_%04d.jpg)", 1},
     { CommandFullResPreview,"-fullpreview","fp", "Run the preview using the still capture resolution (may reduce preview fps)", 0},
     { CommandKeypress,"-keypress",   "k",  "Wait between captures for a ENTER, X then ENTER to exit", 0},
     { CommandSignal,  "-signal",     "s",  "Wait between captures for a SIGUSR1 or SIGUSR2 from another process", 0},
     { CommandGL,      "-gl",         "g",  "Draw preview to texture instead of using video render component", 0},
     { CommandGLCapture, "-glcapture","gc", "Capture the GL frame-buffer instead of the camera image", 0},
     { CommandBurstMode, "-burst",    "bm", "Enable 'burst capture mode'", 0},
     { CommandDateTime,  "-datetime",  "dt", "Replace output pattern (%d) with DateTime (MonthDayHourMinSec)", 0},
     { CommandTimeStamp, "-timestamp", "ts", "Replace output pattern (%d) with unix timestamp (seconds since 1970)", 0},
     { CommandFrameStart,"-framestart","fs",  "Starting frame number in output pattern(%d)", 1},
     { CommandRestartInterval, "-restart","rs","JPEG Restart interval (default of 0 for none)", 1},
  };
  
  static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]);
  
  static struct
  {
     char *format;
     MMAL_FOURCC_T encoding;
  } encoding_xref[] =
  {
     {"jpg", MMAL_ENCODING_JPEG},
     {"bmp", MMAL_ENCODING_BMP},
     {"gif", MMAL_ENCODING_GIF},
     {"png", MMAL_ENCODING_PNG},
     {"ppm", MMAL_ENCODING_PPM},
     {"tga", MMAL_ENCODING_TGA}
  };
  
  static int encoding_xref_size = sizeof(encoding_xref) / sizeof(encoding_xref[0]);
  
  
  static struct
  {
     char *description;
     int nextFrameMethod;
  } next_frame_description[] =
  {
     {"Single capture",         FRAME_NEXT_SINGLE},
     {"Capture on timelapse",   FRAME_NEXT_TIMELAPSE},
     {"Capture on keypress",    FRAME_NEXT_KEYPRESS},
     {"Run forever",            FRAME_NEXT_FOREVER},
     {"Capture on GPIO",        FRAME_NEXT_GPIO},
     {"Capture on signal",      FRAME_NEXT_SIGNAL},
  };
  
  static int next_frame_description_size = sizeof(next_frame_description) / sizeof(next_frame_description[0]);
  
  
  /**
   * Assign a default set of parameters to the state passed in
   *
   * @param state Pointer to state structure to assign defaults to
   */
  static void default_status(RASPISTILL_STATE *state)
  {
     if (!state)
     {
        vcos_assert(0);
        return;
     }
  
     memset(state, 0, sizeof(*state));
  
     raspicommonsettings_set_defaults(&state->common_settings);
  
     state->timeout = -1; // replaced with 5000ms later if unset
     state->quality = 85;
     state->wantRAW = 0;
     state->linkname = NULL;
     state->frameStart = 0;
     state->thumbnailConfig.enable = 1;
     state->thumbnailConfig.width = 64;
     state->thumbnailConfig.height = 48;
     state->thumbnailConfig.quality = 35;
     state->demoMode = 0;
     state->demoInterval = 250; // ms
     state->camera_component = NULL;
     state->encoder_component = NULL;
     state->preview_connection = NULL;
     state->encoder_connection = NULL;
     state->encoder_pool = NULL;
     state->encoding = MMAL_ENCODING_JPEG;
     state->numExifTags = 0;
     state->enableExifTags = 1;
     state->timelapse = 0;
     state->fullResPreview = 0;
     state->frameNextMethod = FRAME_NEXT_SINGLE;
     state->useGL = 0;
     state->glCapture = 0;
     state->burstCaptureMode=0;
     state->datetime = 0;
     state->timestamp = 0;
     state->restart_interval = 0;
  
     // Setup preview window defaults
     raspipreview_set_defaults(&state->preview_parameters);
  
     // Set up the camera_parameters to default
     raspicamcontrol_set_defaults(&state->camera_parameters);
  
     // Set initial GL preview state
     raspitex_set_defaults(&state->raspitex_state);
  }
  
  /**
   * Dump image state parameters to stderr. Used for debugging
   *
   * @param state Pointer to state structure to assign defaults to
   */
  static void dump_status(RASPISTILL_STATE *state)
  {
     int i;
  
     if (!state)
     {
        vcos_assert(0);
        return;
     }
  
     raspicommonsettings_dump_parameters(&state->common_settings);
  
     fprintf(stderr, "Quality %d, Raw %s\n", state->quality, state->wantRAW ? "yes" : "no");
     fprintf(stderr, "Thumbnail enabled %s, width %d, height %d, quality %d\n",
             state->thumbnailConfig.enable ? "Yes":"No", state->thumbnailConfig.width,
             state->thumbnailConfig.height, state->thumbnailConfig.quality);
  
     fprintf(stderr, "Time delay %d, Timelapse %d\n", state->timeout, state->timelapse);
     fprintf(stderr, "Link to latest frame enabled ");
     if (state->linkname)
     {
        fprintf(stderr, " yes, -> %s\n", state->linkname);
     }
     else
     {
        fprintf(stderr, " no\n");
     }
     fprintf(stderr, "Full resolution preview %s\n", state->fullResPreview ? "Yes": "No");
  
     fprintf(stderr, "Capture method : ");
     for (i=0; i<next_frame_description_size; i++)
     {
        if (state->frameNextMethod == next_frame_description[i].nextFrameMethod)
           fprintf(stderr, "%s", next_frame_description[i].description);
     }
     fprintf(stderr, "\n\n");
  
     if (state->enableExifTags)
     {
        if (state->numExifTags)
        {
           fprintf(stderr, "User supplied EXIF tags :\n");
  
           for (i=0; i<state->numExifTags; i++)
           {
              fprintf(stderr, "%s", state->exifTags[i]);
              if (i != state->numExifTags-1)
                 fprintf(stderr, ",");
           }
           fprintf(stderr, "\n\n");
        }
     }
     else
        fprintf(stderr, "EXIF tags disabled\n");
  
     raspipreview_dump_parameters(&state->preview_parameters);
     raspicamcontrol_dump_parameters(&state->camera_parameters);
  }
  
  /**
   * Display usage information for the application to stdout
   *
   * @param app_name String to display as the application name
   */
  static void application_help_message(char *app_name)
  {
     fprintf(stdout, "Runs camera for specific time, and take JPG capture at end if requested\n\n");
     fprintf(stdout, "usage: %s [options]\n\n", app_name);
  
     fprintf(stdout, "Image parameter commands\n\n");
  
     raspicli_display_help(cmdline_commands, cmdline_commands_size);
  
     raspitex_display_help();
  
     return;
  }
  
  /**
   * Parse the incoming command line and put resulting parameters in to the state
   *
   * @param argc Number of arguments in command line
   * @param argv Array of pointers to strings from command line
   * @param state Pointer to state structure to assign any discovered parameters to
   * @return non-0 if failed for some reason, 0 otherwise
   */
  static int parse_cmdline(int argc, const char **argv, RASPISTILL_STATE *state)
  {
     // Parse the command line arguments.
     // We are looking for --<something> or -<abbreviation of something>
  
     int valid = 1;
     int i;
  
     for (i = 1; i < argc && valid; i++)
     {
        int command_id, num_parameters;
  
        if (!argv[i])
           continue;
  
        if (argv[i][0] != '-')
        {
           valid = 0;
           continue;
        }
  
        // Assume parameter is valid until proven otherwise
        valid = 1;
  
        command_id = raspicli_get_command_id(cmdline_commands, cmdline_commands_size, &argv[i][1], &num_parameters);
  
        // If we found a command but are missing a parameter, continue (and we will drop out of the loop)
        if (command_id != -1 && num_parameters > 0 && (i + 1 >= argc) )
           continue;
  
        //  We are now dealing with a command line option
        switch (command_id)
        {
        case CommandQuality: // Quality = 1-100
           if (sscanf(argv[i + 1], "%u", &state->quality) == 1)
           {
              if (state->quality > 100)
              {
                 fprintf(stderr, "Setting max quality = 100\n");
                 state->quality = 100;
              }
              i++;
           }
           else
              valid = 0;
           break;
  
        case CommandRaw: // Add raw bayer data in metadata
           state->wantRAW = 1;
           break;
  
        case CommandLink :
        {
           int len = strlen(argv[i+1]);
           if (len)
           {
              state->linkname = malloc(len + 10);
              vcos_assert(state->linkname);
              if (state->linkname)
                 strncpy(state->linkname, argv[i + 1], len+1);
              i++;
           }
           else
              valid = 0;
           break;
  
        }
  
        case CommandFrameStart:  // use a staring value != 0
        {
           if (sscanf(argv[i + 1], "%d", &state->frameStart) == 1)
           {
              i++;
           }
           else
              valid = 0;
           break;
        }
  
        case CommandDateTime: // use datetime
           state->datetime = 1;
           break;
  
        case CommandTimeStamp: // use timestamp
           state->timestamp = 1;
           break;
  
        case CommandTimeout: // Time to run viewfinder for before taking picture, in seconds
        {
           if (sscanf(argv[i + 1], "%d", &state->timeout) == 1)
           {
              // Ensure that if previously selected CommandKeypress we don't overwrite it
              if (state->timeout == 0 && state->frameNextMethod == FRAME_NEXT_SINGLE)
                 state->frameNextMethod = FRAME_NEXT_FOREVER;
  
              i++;
           }
           else
              valid = 0;
           break;
        }
  
        case CommandThumbnail : // thumbnail parameters - needs string "x:y:quality"
           if ( strcmp( argv[ i + 1 ], "none" ) == 0 )
           {
              state->thumbnailConfig.enable = 0;
           }
           else
           {
              sscanf(argv[i + 1], "%d:%d:%d",
                     &state->thumbnailConfig.width,
                     &state->thumbnailConfig.height,
                     &state->thumbnailConfig.quality);
           }
           i++;
           break;
  
        case CommandDemoMode: // Run in demo mode - no capture
        {
           // Demo mode might have a timing parameter
           // so check if a) we have another parameter, b) its not the start of the next option
           if (i + 1 < argc  && argv[i+1][0] != '-')
           {
              if (sscanf(argv[i + 1], "%u", &state->demoInterval) == 1)
              {
                 // TODO : What limits do we need for timeout?
                 state->demoMode = 1;
                 i++;
              }
              else
                 valid = 0;
           }
           else
           {
              state->demoMode = 1;
           }
  
           break;
        }
  
        case CommandEncoding :
        {
           int len = strlen(argv[i + 1]);
           valid = 0;
  
           if (len)
           {
              int j;
              for (j=0; j<encoding_xref_size; j++)
              {
                 if (strcmp(encoding_xref[j].format, argv[i+1]) == 0)
                 {
                    state->encoding = encoding_xref[j].encoding;
                    valid = 1;
                    i++;
                    break;
                 }
              }
           }
           break;
        }
  
        case CommandExifTag:
           if ( strcmp( argv[ i + 1 ], "none" ) == 0 )
           {
              state->enableExifTags = 0;
           }
           else
           {
              store_exif_tag(state, argv[i+1]);
           }
           i++;
           break;
  
        case CommandTimelapse:
           if (sscanf(argv[i + 1], "%u", &state->timelapse) != 1)
              valid = 0;
           else
           {
              if (state->timelapse)
                 state->frameNextMethod = FRAME_NEXT_TIMELAPSE;
              else
                 state->frameNextMethod = FRAME_NEXT_IMMEDIATELY;
  
              i++;
           }
           break;
  
        case CommandFullResPreview:
           state->fullResPreview = 1;
           break;
  
        case CommandKeypress: // Set keypress between capture mode
           state->frameNextMethod = FRAME_NEXT_KEYPRESS;
  
           if (state->timeout == -1)
              state->timeout = 0;
  
           break;
  
        case CommandSignal:   // Set SIGUSR1 & SIGUSR2 between capture mode
           state->frameNextMethod = FRAME_NEXT_SIGNAL;
           // Reenable the signal
           signal(SIGUSR1, default_signal_handler);
           signal(SIGUSR2, default_signal_handler);
  
           if (state->timeout == -1)
              state->timeout = 0;
  
           break;
  
        case CommandGL:
           state->useGL = 1;
           break;
  
        case CommandGLCapture:
           state->glCapture = 1;
           break;
  
        case CommandBurstMode:
           state->burstCaptureMode=1;
           break;
  
        case CommandRestartInterval:
        {
           if (sscanf(argv[i + 1], "%u", &state->restart_interval) == 1)
           {
              i++;
           }
           else
              valid = 0;
           break;
        }
  
        default:
        {
           // Try parsing for any image specific parameters
           // result indicates how many parameters were used up, 0,1,2
           // but we adjust by -1 as we have used one already
           const char *second_arg = (i + 1 < argc) ? argv[i + 1] : NULL;
           int parms_used = raspicamcontrol_parse_cmdline(&state->camera_parameters, &argv[i][1], second_arg);
  
           // Still unused, try common settings
           if (!parms_used)
              parms_used = raspicommonsettings_parse_cmdline(&state->common_settings, &argv[i][1], second_arg, &application_help_message);
  
           // Still unused, try preview settings
           if (!parms_used)
              parms_used = raspipreview_parse_cmdline(&state->preview_parameters, &argv[i][1], second_arg);
  
           // Still unused, try GL preview options
           if (!parms_used)
              parms_used = raspitex_parse_cmdline(&state->raspitex_state, &argv[i][1], second_arg);
  
           // If no parms were used, this must be a bad parameters
           if (!parms_used)
              valid = 0;
           else
              i += parms_used - 1;
  
           break;
        }
        }
     }
  
     /* GL preview parameters use preview parameters as defaults unless overriden */
     if (! state->raspitex_state.gl_win_defined)
     {
        state->raspitex_state.x       = state->preview_parameters.previewWindow.x;
        state->raspitex_state.y       = state->preview_parameters.previewWindow.y;
        state->raspitex_state.width   = state->preview_parameters.previewWindow.width;
        state->raspitex_state.height  = state->preview_parameters.previewWindow.height;
     }
     /* Also pass the preview information through so GL renderer can determine
      * the real resolution of the multi-media image */
     state->raspitex_state.preview_x       = state->preview_parameters.previewWindow.x;
     state->raspitex_state.preview_y       = state->preview_parameters.previewWindow.y;
     state->raspitex_state.preview_width   = state->preview_parameters.previewWindow.width;
     state->raspitex_state.preview_height  = state->preview_parameters.previewWindow.height;
     state->raspitex_state.opacity         = state->preview_parameters.opacity;
     state->raspitex_state.verbose         = state->common_settings.verbose;
  
     if (!valid)
     {
        fprintf(stderr, "Invalid command line option (%s)\n", argv[i-1]);
        return 1;
     }
  
     return 0;
  }
  
  
  /**
   *  buffer header callback function for encoder
   *
   *  Callback will dump buffer data to the specific file
   *
   * @param port Pointer to port from which callback originated
   * @param buffer mmal buffer header pointer
   */
  static void encoder_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
  {
     int complete = 0;
  
     // We pass our file handle and other stuff in via the userdata field.
  
     PORT_USERDATA *pData = (PORT_USERDATA *)port->userdata;
  
     if (pData)
     {
        int bytes_written = buffer->length;
  
        if (buffer->length && pData->file_handle)
        {
           mmal_buffer_header_mem_lock(buffer);
  
           bytes_written = fwrite(buffer->data, 1, buffer->length, pData->file_handle);
  
           mmal_buffer_header_mem_unlock(buffer);
        }
  
        // We need to check we wrote what we wanted - it's possible we have run out of storage.
        if (bytes_written != buffer->length)
        {
           vcos_log_error("Unable to write buffer to file - aborting");
           complete = 1;
        }
  
        // Now flag if we have completed
        if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED))
           complete = 1;
     }
     else
     {
        vcos_log_error("Received a encoder buffer callback with no state");
     }
  
     // release buffer back to the pool
     mmal_buffer_header_release(buffer);
  
     // and send one back to the port (if still open)
     if (port->is_enabled)
     {
        MMAL_STATUS_T status = MMAL_SUCCESS;
        MMAL_BUFFER_HEADER_T *new_buffer;
  
        new_buffer = mmal_queue_get(pData->pstate->encoder_pool->queue);
  
        if (new_buffer)
        {
           status = mmal_port_send_buffer(port, new_buffer);
        }
        if (!new_buffer || status != MMAL_SUCCESS)
           vcos_log_error("Unable to return a buffer to the encoder port");
     }
  
     if (complete)
        vcos_semaphore_post(&(pData->complete_semaphore));
  }
  
  /**
   * Create the camera component, set up its ports
   *
   * @param state Pointer to state control struct. camera_component member set to the created camera_component if successful.
   *
   * @return MMAL_SUCCESS if all OK, something else otherwise
   *
   */
  static MMAL_STATUS_T create_camera_component(RASPISTILL_STATE *state)
  {
     MMAL_COMPONENT_T *camera = 0;
     MMAL_ES_FORMAT_T *format;
     MMAL_PORT_T *preview_port = NULL, *video_port = NULL, *still_port = NULL;
     MMAL_STATUS_T status;
  
     /* Create the component */
     status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Failed to create camera component");
        goto error;
     }
  
     status = raspicamcontrol_set_stereo_mode(camera->output[0], &state->camera_parameters.stereo_mode);
     status += raspicamcontrol_set_stereo_mode(camera->output[1], &state->camera_parameters.stereo_mode);
     status += raspicamcontrol_set_stereo_mode(camera->output[2], &state->camera_parameters.stereo_mode);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Could not set stereo mode : error %d", status);
        goto error;
     }
  
     MMAL_PARAMETER_INT32_T camera_num =
     {{MMAL_PARAMETER_CAMERA_NUM, sizeof(camera_num)}, state->common_settings.cameraNum};
  
     status = mmal_port_parameter_set(camera->control, &camera_num.hdr);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Could not select camera : error %d", status);
        goto error;
     }
  
     if (!camera->output_num)
     {
        status = MMAL_ENOSYS;
        vcos_log_error("Camera doesn't have output ports");
        goto error;
     }
  
     status = mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_CAMERA_CUSTOM_SENSOR_CONFIG, state->common_settings.sensor_mode);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Could not set sensor mode : error %d", status);
        goto error;
     }
  
     preview_port = camera->output[MMAL_CAMERA_PREVIEW_PORT];
     video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];
     still_port = camera->output[MMAL_CAMERA_CAPTURE_PORT];
  
     // Enable the camera, and tell it its control callback function
     status = mmal_port_enable(camera->control, default_camera_control_callback);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Unable to enable control port : error %d", status);
        goto error;
     }
  
     //  set up the camera configuration
     {
        MMAL_PARAMETER_CAMERA_CONFIG_T cam_config =
        {
           { MMAL_PARAMETER_CAMERA_CONFIG, sizeof(cam_config) },
           .max_stills_w = state->common_settings.width,
           .max_stills_h = state->common_settings.height,
           .stills_yuv422 = 0,
           .one_shot_stills = 1,
           .max_preview_video_w = state->preview_parameters.previewWindow.width,
           .max_preview_video_h = state->preview_parameters.previewWindow.height,
           .num_preview_video_frames = 3,
           .stills_capture_circular_buffer_height = 0,
           .fast_preview_resume = 0,
           .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RESET_STC
        };
  
        if (state->fullResPreview)
        {
           cam_config.max_preview_video_w = state->common_settings.width;
           cam_config.max_preview_video_h = state->common_settings.height;
        }
  
        mmal_port_parameter_set(camera->control, &cam_config.hdr);
     }
  
     raspicamcontrol_set_all_parameters(camera, &state->camera_parameters);
  
     // Now set up the port formats
  
     format = preview_port->format;
     format->encoding = MMAL_ENCODING_OPAQUE;
     format->encoding_variant = MMAL_ENCODING_I420;
  
     if(state->camera_parameters.shutter_speed > 6000000)
     {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
           { 5, 1000 }, {166, 1000}
        };
        mmal_port_parameter_set(preview_port, &fps_range.hdr);
     }
     else if(state->camera_parameters.shutter_speed > 1000000)
     {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
           { 166, 1000 }, {999, 1000}
        };
        mmal_port_parameter_set(preview_port, &fps_range.hdr);
     }
     if (state->fullResPreview)
     {
        // In this mode we are forcing the preview to be generated from the full capture resolution.
        // This runs at a max of 15fps with the OV5647 sensor.
        format->es->video.width = VCOS_ALIGN_UP(state->common_settings.width, 32);
        format->es->video.height = VCOS_ALIGN_UP(state->common_settings.height, 16);
        format->es->video.crop.x = 0;
        format->es->video.crop.y = 0;
        format->es->video.crop.width = state->common_settings.width;
        format->es->video.crop.height = state->common_settings.height;
        format->es->video.frame_rate.num = FULL_RES_PREVIEW_FRAME_RATE_NUM;
        format->es->video.frame_rate.den = FULL_RES_PREVIEW_FRAME_RATE_DEN;
     }
     else
     {
        // Use a full FOV 4:3 mode
        format->es->video.width = VCOS_ALIGN_UP(state->preview_parameters.previewWindow.width, 32);
        format->es->video.height = VCOS_ALIGN_UP(state->preview_parameters.previewWindow.height, 16);
        format->es->video.crop.x = 0;
        format->es->video.crop.y = 0;
        format->es->video.crop.width = state->preview_parameters.previewWindow.width;
        format->es->video.crop.height = state->preview_parameters.previewWindow.height;
        format->es->video.frame_rate.num = PREVIEW_FRAME_RATE_NUM;
        format->es->video.frame_rate.den = PREVIEW_FRAME_RATE_DEN;
     }
  
     status = mmal_port_format_commit(preview_port);
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("camera viewfinder format couldn't be set");
        goto error;
     }
  
     // Set the same format on the video  port (which we don't use here)
     mmal_format_full_copy(video_port->format, format);
     status = mmal_port_format_commit(video_port);
  
     if (status  != MMAL_SUCCESS)
     {
        vcos_log_error("camera video format couldn't be set");
        goto error;
     }
  
     // Ensure there are enough buffers to avoid dropping frames
     if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
        video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
  
     format = still_port->format;
  
     if(state->camera_parameters.shutter_speed > 6000000)
     {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
           { 5, 1000 }, {166, 1000}
        };
        mmal_port_parameter_set(still_port, &fps_range.hdr);
     }
     else if(state->camera_parameters.shutter_speed > 1000000)
     {
        MMAL_PARAMETER_FPS_RANGE_T fps_range = {{MMAL_PARAMETER_FPS_RANGE, sizeof(fps_range)},
           { 167, 1000 }, {999, 1000}
        };
        mmal_port_parameter_set(still_port, &fps_range.hdr);
     }
     // Set our stills format on the stills (for encoder) port
     format->encoding = MMAL_ENCODING_OPAQUE;
     format->es->video.width = VCOS_ALIGN_UP(state->common_settings.width, 32);
     format->es->video.height = VCOS_ALIGN_UP(state->common_settings.height, 16);
     format->es->video.crop.x = 0;
     format->es->video.crop.y = 0;
     format->es->video.crop.width = state->common_settings.width;
     format->es->video.crop.height = state->common_settings.height;
     format->es->video.frame_rate.num = STILLS_FRAME_RATE_NUM;
     format->es->video.frame_rate.den = STILLS_FRAME_RATE_DEN;
  
     status = mmal_port_format_commit(still_port);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("camera still format couldn't be set");
        goto error;
     }
  
     /* Ensure there are enough buffers to avoid dropping frames */
     if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM)
        still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
  
     /* Enable component */
     status = mmal_component_enable(camera);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("camera component couldn't be enabled");
        goto error;
     }
  
     if (state->useGL)
     {
        status = raspitex_configure_preview_port(&state->raspitex_state, preview_port);
        if (status != MMAL_SUCCESS)
        {
           fprintf(stderr, "Failed to configure preview port for GL rendering");
           goto error;
        }
     }
  
     state->camera_component = camera;
  
     if (state->common_settings.verbose)
        fprintf(stderr, "Camera component done\n");
  
     return status;
  
  error:
  
     if (camera)
        mmal_component_destroy(camera);
  
     return status;
  }
  
  /**
   * Destroy the camera component
   *
   * @param state Pointer to state control struct
   *
   */
  static void destroy_camera_component(RASPISTILL_STATE *state)
  {
     if (state->camera_component)
     {
        mmal_component_destroy(state->camera_component);
        state->camera_component = NULL;
     }
  }
  
  /**
   * Create the encoder component, set up its ports
   *
   * @param state Pointer to state control struct. encoder_component member set to the created camera_component if successful.
   *
   * @return a MMAL_STATUS, MMAL_SUCCESS if all OK, something else otherwise
   */
  static MMAL_STATUS_T create_encoder_component(RASPISTILL_STATE *state)
  {
     MMAL_COMPONENT_T *encoder = 0;
     MMAL_PORT_T *encoder_input = NULL, *encoder_output = NULL;
     MMAL_STATUS_T status;
     MMAL_POOL_T *pool;
  
     status = mmal_component_create(MMAL_COMPONENT_DEFAULT_IMAGE_ENCODER, &encoder);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Unable to create JPEG encoder component");
        goto error;
     }
  
     if (!encoder->input_num || !encoder->output_num)
     {
        status = MMAL_ENOSYS;
        vcos_log_error("JPEG encoder doesn't have input/output ports");
        goto error;
     }
  
     encoder_input = encoder->input[0];
     encoder_output = encoder->output[0];
  
     // We want same format on input and output
     mmal_format_copy(encoder_output->format, encoder_input->format);
  
     // Specify out output format
     encoder_output->format->encoding = state->encoding;
  
     encoder_output->buffer_size = encoder_output->buffer_size_recommended;
  
     if (encoder_output->buffer_size < encoder_output->buffer_size_min)
        encoder_output->buffer_size = encoder_output->buffer_size_min;
  
     encoder_output->buffer_num = encoder_output->buffer_num_recommended;
  
     if (encoder_output->buffer_num < encoder_output->buffer_num_min)
        encoder_output->buffer_num = encoder_output->buffer_num_min;
  
     // Commit the port changes to the output port
     status = mmal_port_format_commit(encoder_output);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Unable to set format on video encoder output port");
        goto error;
     }
  
     // Set the JPEG quality level
     status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_Q_FACTOR, state->quality);
  
     if (status != MMAL_SUCCESS)
     {
        vcos_log_error("Unable to set JPEG quality");
        goto error;
     }
  
     // Set the JPEG restart interval
     status = mmal_port_parameter_set_uint32(encoder_output, MMAL_PARAMETER_JPEG_RESTART_INTERVAL, state->restart_interval);
  
     if (state->restart_interval && status != MMAL_SUCCESS)
     {
        vcos_log_error("Unable to set JPEG restart interval");
        goto error;
     }
  
     // Set up any required thumbnail
     {
        MMAL_PARAMETER_THUMBNAIL_CONFIG_T param_thumb = {{MMAL_PARAMETER_THUMBNAIL_CONFIGURATION, sizeof(MMAL_PARAMETER_THUMBNAIL_CONFIG_T)}, 0, 0, 0, 0};
  
        if ( state->thumbnailConfig.enable &&
              state->thumbnailConfig.width > 0 && state->thumbnailConfig.height > 0 )
        {
           // Have a valid thumbnail defined
           param_thumb.enable = 1;
           param_thumb.width = state->thumbnailConfig.width;
           param_thumb.height = state->thumbnailConfig.height;
           param_thumb.quality = state->thumbnailConfig.quality;
        }
        status = mmal_port_parameter_set(encoder->control, &param_thumb.hdr);
     }
  
     //  Enable component
     status = mmal_component_enable(encoder);
  
     if (status  != MMAL_SUCCESS)
     {
        vcos_log_error("Unable to enable video encoder component");
        goto error;
     }
  
     /* Create pool of buffer headers for the output port to consume */
     pool = mmal_port_pool_create(encoder_output, encoder_output->buffer_num, encoder_output->buffer_size);
  
     if (!pool)
     {
        vcos_log_error("Failed to create buffer header pool for encoder output port %s", encoder_output->name);
     }
  
     state->encoder_pool = pool;
     state->encoder_component = encoder;
  
     if (state->common_settings.verbose)
        fprintf(stderr, "Encoder component done\n");
  
     return status;
  
  error:
  
     if (encoder)
        mmal_component_destroy(encoder);
  
     return status;
  }
  
  /**
   * Destroy the encoder component
   *
   * @param state Pointer to state control struct
   *
   */
  static void destroy_encoder_component(RASPISTILL_STATE *state)
  {
     // Get rid of any port buffers first
     if (state->encoder_pool)
     {
        mmal_port_pool_destroy(state->encoder_component->output[0], state->encoder_pool);
     }
  
     if (state->encoder_component)
     {
        mmal_component_destroy(state->encoder_component);
        state->encoder_component = NULL;
     }
  }
  
  
  /**
   * Add an exif tag to the capture
   *
   * @param state Pointer to state control struct
   * @param exif_tag String containing a "key=value" pair.
   * @return  Returns a MMAL_STATUS_T giving result of operation
   */
  static MMAL_STATUS_T add_exif_tag(RASPISTILL_STATE *state, const char *exif_tag)
  {
     MMAL_STATUS_T status;
     MMAL_PARAMETER_EXIF_T *exif_param = (MMAL_PARAMETER_EXIF_T*)calloc(sizeof(MMAL_PARAMETER_EXIF_T) + MAX_EXIF_PAYLOAD_LENGTH, 1);
  
     vcos_assert(state);
     vcos_assert(state->encoder_component);
  
     // Check to see if the tag is present or is indeed a key=value pair.
     if (!exif_tag || strchr(exif_tag, '=') == NULL || strlen(exif_tag) > MAX_EXIF_PAYLOAD_LENGTH-1)
        return MMAL_EINVAL;
  
     exif_param->hdr.id = MMAL_PARAMETER_EXIF;
  
     strncpy((char*)exif_param->data, exif_tag, MAX_EXIF_PAYLOAD_LENGTH-1);
  
     exif_param->hdr.size = sizeof(MMAL_PARAMETER_EXIF_T) + strlen((char*)exif_param->data);
  
     status = mmal_port_parameter_set(state->encoder_component->output[0], &exif_param->hdr);
  
     free(exif_param);
  
     return status;
  }
  
  /**
   * Add a basic set of EXIF tags to the capture
   * Make, Time etc
   *
   * @param state Pointer to state control struct
   *
   */
  static void add_exif_tags(RASPISTILL_STATE *state, struct gps_data_t *gpsdata)
  {
     time_t rawtime;
     struct tm *timeinfo;
     char model_buf[32];
     char time_buf[32];
     char exif_buf[128];
     int i;
  
     snprintf(model_buf, 32, "IFD0.Model=RP_%s", state->common_settings.camera_name);
     add_exif_tag(state, model_buf);
     add_exif_tag(state, "IFD0.Make=RaspberryPi");
  
     time(&rawtime);
     timeinfo = localtime(&rawtime);
  
     snprintf(time_buf, sizeof(time_buf),
              "%04d:%02d:%02d %02d:%02d:%02d",
              timeinfo->tm_year+1900,
              timeinfo->tm_mon+1,
              timeinfo->tm_mday,
              timeinfo->tm_hour,
              timeinfo->tm_min,
              timeinfo->tm_sec);
  
     snprintf(exif_buf, sizeof(exif_buf), "EXIF.DateTimeDigitized=%s", time_buf);
     add_exif_tag(state, exif_buf);
  
     snprintf(exif_buf, sizeof(exif_buf), "EXIF.DateTimeOriginal=%s", time_buf);
     add_exif_tag(state, exif_buf);
  
     snprintf(exif_buf, sizeof(exif_buf), "IFD0.DateTime=%s", time_buf);
     add_exif_tag(state, exif_buf);
  
  
     // Add GPS tags
     if (state->common_settings.gps)
     {
        // clear all existing tags first
        add_exif_tag(state, "GPS.GPSDateStamp=");
        add_exif_tag(state, "GPS.GPSTimeStamp=");
        add_exif_tag(state, "GPS.GPSMeasureMode=");
        add_exif_tag(state, "GPS.GPSSatellites=");
        add_exif_tag(state, "GPS.GPSLatitude=");
        add_exif_tag(state, "GPS.GPSLatitudeRef=");
        add_exif_tag(state, "GPS.GPSLongitude=");
        add_exif_tag(state, "GPS.GPSLongitudeRef=");
        add_exif_tag(state, "GPS.GPSAltitude=");
        add_exif_tag(state, "GPS.GPSAltitudeRef=");
        add_exif_tag(state, "GPS.GPSSpeed=");
        add_exif_tag(state, "GPS.GPSSpeedRef=");
        add_exif_tag(state, "GPS.GPSTrack=");
        add_exif_tag(state, "GPS.GPSTrackRef=");
  
        if (gpsdata->online)
        {
           if (state->common_settings.verbose)
              fprintf(stderr, "Adding GPS EXIF\n");
           if (gpsdata->set & TIME_SET)
           {
              rawtime = gpsdata->fix.time;
              timeinfo = localtime(&rawtime);
              strftime(time_buf, sizeof(time_buf), "%Y:%m:%d", timeinfo);
              snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSDateStamp=%s", time_buf);
              add_exif_tag(state, exif_buf);
              strftime(time_buf, sizeof(time_buf), "%H/1,%M/1,%S/1", timeinfo);
              snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSTimeStamp=%s", time_buf);
              add_exif_tag(state, exif_buf);
           }
           if (gpsdata->fix.mode >= MODE_2D)
           {
              snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSMeasureMode=%c",
                       (gpsdata->fix.mode >= MODE_3D) ? '3' : '2');
              add_exif_tag(state, exif_buf);
              if ((gpsdata->satellites_used > 0) && (gpsdata->satellites_visible > 0))
              {
                 snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSatellites=Used:%d,Visible:%d",
                          gpsdata->satellites_used, gpsdata->satellites_visible);
                 add_exif_tag(state, exif_buf);
              }
              else if (gpsdata->satellites_used > 0)
              {
                 snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSatellites=Used:%d",
                          gpsdata->satellites_used);
                 add_exif_tag(state, exif_buf);
              }
              else if (gpsdata->satellites_visible > 0)
              {
                 snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSatellites=Visible:%d",
                          gpsdata->satellites_visible);
                 add_exif_tag(state, exif_buf);
              }
  
              if (gpsdata->set & LATLON_SET)
              {
                 if (isnan(gpsdata->fix.latitude) == 0)
                 {
                    if (deg_to_str(fabs(gpsdata->fix.latitude), time_buf, sizeof(time_buf)) == 0)
                    {
                       snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLatitude=%s", time_buf);
                       add_exif_tag(state, exif_buf);
                       snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLatitudeRef=%c",
                                (gpsdata->fix.latitude < 0) ? 'S' : 'N');
                       add_exif_tag(state, exif_buf);
                    }
                 }
                 if (isnan(gpsdata->fix.longitude) == 0)
                 {
                    if (deg_to_str(fabs(gpsdata->fix.longitude), time_buf, sizeof(time_buf)) == 0)
                    {
                       snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLongitude=%s", time_buf);
                       add_exif_tag(state, exif_buf);
                       snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSLongitudeRef=%c",
                                (gpsdata->fix.longitude < 0) ? 'W' : 'E');
                       add_exif_tag(state, exif_buf);
                    }
                 }
              }
              if ((gpsdata->set & ALTITUDE_SET) && (gpsdata->fix.mode >= MODE_3D))
              {
                 if (isnan(gpsdata->fix.altitude) == 0)
                 {
                    snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSAltitude=%d/10",
                             (int)(gpsdata->fix.altitude*10+0.5));
                    add_exif_tag(state, exif_buf);
                    add_exif_tag(state, "GPS.GPSAltitudeRef=0");
                 }
              }
              if (gpsdata->set & SPEED_SET)
              {
                 if (isnan(gpsdata->fix.speed) == 0)
                 {
                    snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSSpeed=%d/10",
                             (int)(gpsdata->fix.speed*MPS_TO_KPH*10+0.5));
                    add_exif_tag(state, exif_buf);
                    add_exif_tag(state, "GPS.GPSSpeedRef=K");
                 }
              }
              if (gpsdata->set & TRACK_SET)
              {
                 if (isnan(gpsdata->fix.track) == 0)
                 {
                    snprintf(exif_buf, sizeof(exif_buf), "GPS.GPSTrack=%d/100",
                             (int)(gpsdata->fix.track*100+0.5));
                    add_exif_tag(state, exif_buf);
                    add_exif_tag(state, "GPS.GPSTrackRef=T");
                 }
              }
           }
        }
     }
  
     // Now send any user supplied tags
  
     for (i=0; i<state->numExifTags && i < MAX_USER_EXIF_TAGS; i++)
     {
        if (state->exifTags[i])
        {
           add_exif_tag(state, state->exifTags[i]);
        }
     }
  }
  
  /**
   * Stores an EXIF tag in the state, incrementing various pointers as necessary.
   * Any tags stored in this way will be added to the image file when add_exif_tags
   * is called
   *
   * Will not store if run out of storage space
   *
   * @param state Pointer to state control struct
   * @param exif_tag EXIF tag string
   *
   */
  static void store_exif_tag(RASPISTILL_STATE *state, const char *exif_tag)
  {
     if (state->numExifTags < MAX_USER_EXIF_TAGS)
     {
        state->exifTags[state->numExifTags] = exif_tag;
        state->numExifTags++;
     }
  }
  
  /**
   * Allocates and generates a filename based on the
   * user-supplied pattern and the frame number.
   * On successful return, finalName and tempName point to malloc()ed strings
   * which must be freed externally.  (On failure, returns nulls that
   * don't need free()ing.)
   *
   * @param finalName pointer receives an
   * @param pattern sprintf pattern with %d to be replaced by frame
   * @param frame for timelapse, the frame number
   * @return Returns a MMAL_STATUS_T giving result of operation
  */
  
  MMAL_STATUS_T create_filenames(char** finalName, char** tempName, char * pattern, int frame)
  {
     *finalName = NULL;
     *tempName = NULL;
     if (0 > asprintf(finalName, pattern, frame) ||
           0 > asprintf(tempName, "%s~", *finalName))
     {
        if (*finalName != NULL)
        {
           free(*finalName);
        }
        return MMAL_ENOMEM;    // It may be some other error, but it is not worth getting it right
     }
     return MMAL_SUCCESS;
  }
  
  /**
   * Function to wait in various ways (depending on settings) for the next frame
   *
   * @param state Pointer to the state data
   * @param [in][out] frame The last frame number, adjusted to next frame number on output
   * @return !0 if to continue, 0 if reached end of run
   */
  static int wait_for_next_frame(RASPISTILL_STATE *state, int *frame)
  {
     static int64_t complete_time = -1;
     int keep_running = 1;
  
     int64_t current_time =  get_microseconds64()/1000;
  
     if (complete_time == -1)
        complete_time =  current_time + state->timeout;
  
     // if we have run out of time, flag we need to exit
     // If timeout = 0 then always continue
     if (current_time >= complete_time && state->timeout != 0)
        keep_running = 0;
  
     switch (state->frameNextMethod)
     {
     case FRAME_NEXT_SINGLE :
        // simple timeout for a single capture
        vcos_sleep(state->timeout);
        return 0;
  
     case FRAME_NEXT_FOREVER :
     {
        *frame+=1;
  
        // Have a sleep so we don't hog the CPU.
        vcos_sleep(10000);
  
        // Run forever so never indicate end of loop
        return 1;
     }
  
     case FRAME_NEXT_TIMELAPSE :
     {
        static int64_t next_frame_ms = -1;
  
        // Always need to increment by at least one, may add a skip later
        *frame += 1;
  
        if (next_frame_ms == -1)
        {
           vcos_sleep(CAMERA_SETTLE_TIME);
  
           // Update our current time after the sleep
           current_time =  get_microseconds64()/1000;
  
           // Set our initial 'next frame time'
           next_frame_ms = current_time + state->timelapse;
        }
        else
        {
           int64_t this_delay_ms = next_frame_ms - current_time;
  
           if (this_delay_ms < 0)
           {
              // We are already past the next exposure time
              if (-this_delay_ms < state->timelapse/2)
              {
                 // Less than a half frame late, take a frame and hope to catch up next time
                 next_frame_ms += state->timelapse;
                 vcos_log_error("Frame %d is %d ms late", *frame, (int)(-this_delay_ms));
              }
              else
              {
                 int nskip = 1 + (-this_delay_ms)/state->timelapse;
                 vcos_log_error("Skipping frame %d to restart at frame %d", *frame, *frame+nskip);
                 *frame += nskip;
                 this_delay_ms += nskip * state->timelapse;
                 vcos_sleep(this_delay_ms);
                 next_frame_ms += (nskip + 1) * state->timelapse;
              }
           }
           else
           {
              vcos_sleep(this_delay_ms);
              next_frame_ms += state->timelapse;
           }
        }
  
        return keep_running;
     }
  
     case FRAME_NEXT_KEYPRESS :
     {
        int ch;
  
        if (state->common_settings.verbose)
           fprintf(stderr, "Press Enter to capture, X then ENTER to exit\n");
  
        ch = getchar();
        *frame+=1;
        if (ch == 'x' || ch == 'X')
           return 0;
        else
        {
           return keep_running;
        }
     }
  
     case FRAME_NEXT_IMMEDIATELY :
     {
        // Not waiting, just go to next frame.
        // Actually, we do need a slight delay here otherwise exposure goes
        // badly wrong since we never allow it frames to work it out
        // This could probably be tuned down.
        // First frame has a much longer delay to ensure we get exposure to a steady state
        if (*frame == 0)
           vcos_sleep(CAMERA_SETTLE_TIME);
        else
           vcos_sleep(30);
  
        *frame+=1;
  
        return keep_running;
     }
  
     case FRAME_NEXT_GPIO :
     {
        // Intended for GPIO firing of a capture
        return 0;
     }
  
     case FRAME_NEXT_SIGNAL :
     {
        // Need to wait for a SIGUSR1 or SIGUSR2 signal
        sigset_t waitset;
        int sig;
        int result = 0;
  
        sigemptyset( &waitset );
        sigaddset( &waitset, SIGUSR1 );
        sigaddset( &waitset, SIGUSR2 );
  
        // We are multi threaded because we use mmal, so need to use the pthread
        // variant of procmask to block until a SIGUSR1 or SIGUSR2 signal appears
        pthread_sigmask( SIG_BLOCK, &waitset, NULL );
  
        if (state->common_settings.verbose)
        {
           fprintf(stderr, "Waiting for SIGUSR1 to initiate capture and continue or SIGUSR2 to capture and exit\n");
        }
  
        result = sigwait( &waitset, &sig );
  
        if (result == 0)
        {
           if (sig == SIGUSR1)
           {
              if (state->common_settings.verbose)
                 fprintf(stderr, "Received SIGUSR1\n");
           }
           else if (sig == SIGUSR2)
           {
              if (state->common_settings.verbose)
                 fprintf(stderr, "Received SIGUSR2\n");
              keep_running = 0;
           }
        }
        else
        {
           if (state->common_settings.verbose)
              fprintf(stderr, "Bad signal received - error %d\n", errno);
        }
  
        *frame+=1;
  
        return keep_running;
     }
     } // end of switch
  
     // Should have returned by now, but default to timeout
     return keep_running;
  }
  
  static void rename_file(RASPISTILL_STATE *state, FILE *output_file,
                          const char *final_filename, const char *use_filename, int frame)
  {
     MMAL_STATUS_T status;
  
     fclose(output_file);
     vcos_assert(use_filename != NULL && final_filename != NULL);
     if (0 != rename(use_filename, final_filename))
     {
        vcos_log_error("Could not rename temp file to: %s; %s",
                       final_filename,strerror(errno));
     }
     if (state->linkname)
     {
        char *use_link;
        char *final_link;
        status = create_filenames(&final_link, &use_link, state->linkname, frame);
  
        // Create hard link if possible, symlink otherwise
        if (status != MMAL_SUCCESS
              || (0 != link(final_filename, use_link)
                  &&  0 != symlink(final_filename, use_link))
              || 0 != rename(use_link, final_link))
        {
           vcos_log_error("Could not link as filename: %s; %s",
                          state->linkname,strerror(errno));
        }
        if (use_link) free(use_link);
        if (final_link) free(final_link);
     }
  }
  
  
  /**
   * main
   */
  int main(int argc, const char **argv)
  {
     // Our main data storage vessel..
     RASPISTILL_STATE state;
     int exit_code = EX_OK;
  
     MMAL_STATUS_T status = MMAL_SUCCESS;
     MMAL_PORT_T *camera_preview_port = NULL;
     MMAL_PORT_T *camera_video_port = NULL;
     MMAL_PORT_T *camera_still_port = NULL;
     MMAL_PORT_T *preview_input_port = NULL;
     MMAL_PORT_T *encoder_input_port = NULL;
     MMAL_PORT_T *encoder_output_port = NULL;
  
     bcm_host_init();
  
     // Register our application with the logging system
     vcos_log_register("RaspiStill", VCOS_LOG_CATEGORY);
  
     signal(SIGINT, default_signal_handler);
  
     // Disable USR1 and USR2 for the moment - may be reenabled if go in to signal capture mode
     signal(SIGUSR1, SIG_IGN);
     signal(SIGUSR2, SIG_IGN);
  
     set_app_name(argv[0]);
  
     // Do we have any parameters
     if (argc == 1)
     {
        display_valid_parameters(basename(argv[0]), &application_help_message);
        exit(EX_USAGE);
     }
  
     default_status(&state);
  
     // Parse the command line and put options in to our status structure
     if (parse_cmdline(argc, argv, &state))
     {
        exit(EX_USAGE);
     }
  
     if (state.timeout == -1)
        state.timeout = 5000;
  
     // Setup for sensor specific parameters
     get_sensor_defaults(state.common_settings.cameraNum, state.common_settings.camera_name,
                         &state.common_settings.width, &state.common_settings.height);
  
     if (state.common_settings.verbose)
     {
        print_app_details(stderr);
        dump_status(&state);
     }
  
     if (state.common_settings.gps)
     {
        if (raspi_gps_setup(state.common_settings.verbose))
           state.common_settings.gps = false;
     }
  
     if (state.useGL)
        raspitex_init(&state.raspitex_state);
  
     // OK, we have a nice set of parameters. Now set up our components
     // We have three components. Camera, Preview and encoder.
     // Camera and encoder are different in stills/video, but preview
     // is the same so handed off to a separate module
  
     if ((status = create_camera_component(&state)) != MMAL_SUCCESS)
     {
        vcos_log_error("%s: Failed to create camera component", __func__);
        exit_code = EX_SOFTWARE;
     }
     else if ((!state.useGL) && (status = raspipreview_create(&state.preview_parameters)) != MMAL_SUCCESS)
     {
        vcos_log_error("%s: Failed to create preview component", __func__);
        destroy_camera_component(&state);
        exit_code = EX_SOFTWARE;
     }
     else if ((status = create_encoder_component(&state)) != MMAL_SUCCESS)
     {
        vcos_log_error("%s: Failed to create encode component", __func__);
        raspipreview_destroy(&state.preview_parameters);
        destroy_camera_component(&state);
        exit_code = EX_SOFTWARE;
     }
     else
     {
        PORT_USERDATA callback_data;
  
        if (state.common_settings.verbose)
           fprintf(stderr, "Starting component connection stage\n");
  
        camera_preview_port = state.camera_component->output[MMAL_CAMERA_PREVIEW_PORT];
        camera_video_port   = state.camera_component->output[MMAL_CAMERA_VIDEO_PORT];
        camera_still_port   = state.camera_component->output[MMAL_CAMERA_CAPTURE_PORT];
        encoder_input_port  = state.encoder_component->input[0];
        encoder_output_port = state.encoder_component->output[0];
  
        if (! state.useGL)
        {
           if (state.common_settings.verbose)
              fprintf(stderr, "Connecting camera preview port to video render.\n");
  
           // Note we are lucky that the preview and null sink components use the same input port
           // so we can simple do this without conditionals
           preview_input_port  = state.preview_parameters.preview_component->input[0];
  
           // Connect camera to preview (which might be a null_sink if no preview required)
           status = connect_ports(camera_preview_port, preview_input_port, &state.preview_connection);
        }
  
        if (status == MMAL_SUCCESS)
        {
           VCOS_STATUS_T vcos_status;
  
           if (state.common_settings.verbose)
              fprintf(stderr, "Connecting camera stills port to encoder input port\n");
  
           // Now connect the camera to the encoder
           status = connect_ports(camera_still_port, encoder_input_port, &state.encoder_connection);
  
           if (status != MMAL_SUCCESS)
           {
              vcos_log_error("%s: Failed to connect camera video port to encoder input", __func__);
              goto error;
           }
  
           // Set up our userdata - this is passed though to the callback where we need the information.
           // Null until we open our filename
           callback_data.file_handle = NULL;
           callback_data.pstate = &state;
           vcos_status = vcos_semaphore_create(&callback_data.complete_semaphore, "RaspiStill-sem", 0);
  
           vcos_assert(vcos_status == VCOS_SUCCESS);
  
           /* If GL preview is requested then start the GL threads */
           if (state.useGL && (raspitex_start(&state.raspitex_state) != 0))
              goto error;
  
           if (status != MMAL_SUCCESS)
           {
              vcos_log_error("Failed to setup encoder output");
              goto error;
           }
  
           if (state.demoMode)
           {
              // Run for the user specific time..
              int num_iterations = state.timeout / state.demoInterval;
              int i;
              for (i=0; i<num_iterations; i++)
              {
                 raspicamcontrol_cycle_test(state.camera_component);
                 vcos_sleep(state.demoInterval);
              }
           }
           else
           {
              int frame, keep_looping = 1;
              FILE *output_file = NULL;
              char *use_filename = NULL;      // Temporary filename while image being written
              char *final_filename = NULL;    // Name that file gets once writing complete
  
              frame = state.frameStart - 1;
  
              while (keep_looping)
              {
                 keep_looping = wait_for_next_frame(&state, &frame);
  
                 if (state.datetime)
                 {
                    time_t rawtime;
                    struct tm *timeinfo;
  
                    time(&rawtime);
                    timeinfo = localtime(&rawtime);
  
                    frame = timeinfo->tm_mon+1;
                    frame *= 100;
                    frame += timeinfo->tm_mday;
                    frame *= 100;
                    frame += timeinfo->tm_hour;
                    frame *= 100;
                    frame += timeinfo->tm_min;
                    frame *= 100;
                    frame += timeinfo->tm_sec;
                 }
                 if (state.timestamp)
                 {
                    frame = (int)time(NULL);
                 }
  
                 // Open the file
                 if (state.common_settings.filename)
                 {
                    if (state.common_settings.filename[0] == '-')
                    {
                       output_file = stdout;
                    }
                    else
                    {
                       vcos_assert(use_filename == NULL && final_filename == NULL);
                       status = create_filenames(&final_filename, &use_filename, state.common_settings.filename, frame);
                       if (status  != MMAL_SUCCESS)
                       {
                          vcos_log_error("Unable to create filenames");
                          goto error;
                       }
  
                       if (state.common_settings.verbose)
                          fprintf(stderr, "Opening output file %s\n", final_filename);
                       // Technically it is opening the temp~ filename which will be renamed to the final filename
  
                       output_file = fopen(use_filename, "wb");
  
                       if (!output_file)
                       {
                          // Notify user, carry on but discarding encoded output buffers
                          vcos_log_error("%s: Error opening output file: %s\nNo output file will be generated\n", __func__, use_filename);
                       }
                    }
  
                    callback_data.file_handle = output_file;
                 }
  
                 // We only capture if a filename was specified and it opened
                 if (state.useGL && state.glCapture && output_file)
                 {
                    /* Save the next GL framebuffer as the next camera still */
                    int rc = raspitex_capture(&state.raspitex_state, output_file);
                    if (rc != 0)
                       vcos_log_error("Failed to capture GL preview");
                    rename_file(&state, output_file, final_filename, use_filename, frame);
                 }
                 else if (output_file)
                 {
                    int num, q;
  
                    // Must do this before the encoder output port is enabled since
                    // once enabled no further exif data is accepted
                    if ( state.enableExifTags )
                    {
                       struct gps_data_t *gps_data = raspi_gps_lock();
                       add_exif_tags(&state, gps_data);
                       raspi_gps_unlock();
                    }
                    else
                    {
                       mmal_port_parameter_set_boolean(
                          state.encoder_component->output[0], MMAL_PARAMETER_EXIF_DISABLE, 1);
                    }
  
                    // Same with raw, apparently need to set it for each capture, whilst port
                    // is not enabled
                    if (state.wantRAW)
                    {
                       if (mmal_port_parameter_set_boolean(camera_still_port, MMAL_PARAMETER_ENABLE_RAW_CAPTURE, 1) != MMAL_SUCCESS)
                       {
                          vcos_log_error("RAW was requested, but failed to enable");
                       }
                    }
  
                    // There is a possibility that shutter needs to be set each loop.
                    if (mmal_status_to_int(mmal_port_parameter_set_uint32(state.camera_component->control, MMAL_PARAMETER_SHUTTER_SPEED, state.camera_parameters.shutter_speed)) != MMAL_SUCCESS)
                       vcos_log_error("Unable to set shutter speed");
  
                    // Enable the encoder output port
                    encoder_output_port->userdata = (struct MMAL_PORT_USERDATA_T *)&callback_data;
  
                    if (state.common_settings.verbose)
                       fprintf(stderr, "Enabling encoder output port\n");
  
                    // Enable the encoder output port and tell it its callback function
                    status = mmal_port_enable(encoder_output_port, encoder_buffer_callback);
  
                    // Send all the buffers to the encoder output port
                    num = mmal_queue_length(state.encoder_pool->queue);
  
                    for (q=0; q<num; q++)
                    {
                       MMAL_BUFFER_HEADER_T *buffer = mmal_queue_get(state.encoder_pool->queue);
  
                       if (!buffer)
                          vcos_log_error("Unable to get a required buffer %d from pool queue", q);
  
                       if (mmal_port_send_buffer(encoder_output_port, buffer)!= MMAL_SUCCESS)
                          vcos_log_error("Unable to send a buffer to encoder output port (%d)", q);
                    }
  
                    if (state.burstCaptureMode)
                    {
                       mmal_port_parameter_set_boolean(state.camera_component->control,  MMAL_PARAMETER_CAMERA_BURST_CAPTURE, 1);
                    }
  
                    if(state.camera_parameters.enable_annotate)
                    {
                       if ((state.camera_parameters.enable_annotate & ANNOTATE_APP_TEXT) && state.common_settings.gps)
                       {
                          char *text = raspi_gps_location_string();
                          raspicamcontrol_set_annotate(state.camera_component, state.camera_parameters.enable_annotate,
                                                       text,
                                                       state.camera_parameters.annotate_text_size,
                                                       state.camera_parameters.annotate_text_colour,
                                                       state.camera_parameters.annotate_bg_colour,
                                                       state.camera_parameters.annotate_justify,
                                                       state.camera_parameters.annotate_x,
                                                       state.camera_parameters.annotate_y
                                                      );
                          free(text);
                       }
                       else
                          raspicamcontrol_set_annotate(state.camera_component, state.camera_parameters.enable_annotate,
                                                       state.camera_parameters.annotate_string,
                                                       state.camera_parameters.annotate_text_size,
                                                       state.camera_parameters.annotate_text_colour,
                                                       state.camera_parameters.annotate_bg_colour,
                                                       state.camera_parameters.annotate_justify,
                                                       state.camera_parameters.annotate_x,
                                                       state.camera_parameters.annotate_y
                                                      );
                    }
  
                    if (state.common_settings.verbose)
                       fprintf(stderr, "Starting capture %d\n", frame);
  
                    if (mmal_port_parameter_set_boolean(camera_still_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS)
                    {
                       vcos_log_error("%s: Failed to start capture", __func__);
                    }
                    else
                    {
                       // Wait for capture to complete
                       // For some reason using vcos_semaphore_wait_timeout sometimes returns immediately with bad parameter error
                       // even though it appears to be all correct, so reverting to untimed one until figure out why its erratic
                       vcos_semaphore_wait(&callback_data.complete_semaphore);
                       if (state.common_settings.verbose)
                          fprintf(stderr, "Finished capture %d\n", frame);
                    }
  
                    // Ensure we don't die if get callback with no open file
                    callback_data.file_handle = NULL;
  
                    if (output_file != stdout)
                    {
                       rename_file(&state, output_file, final_filename, use_filename, frame);
                    }
                    else
                    {
                       fflush(output_file);
                    }
                    // Disable encoder output port
                    status = mmal_port_disable(encoder_output_port);
                 }
  
                 if (use_filename)
                 {
                    free(use_filename);
                    use_filename = NULL;
                 }
                 if (final_filename)
                 {
                    free(final_filename);
                    final_filename = NULL;
                 }
              } // end for (frame)
  
              vcos_semaphore_delete(&callback_data.complete_semaphore);
           }
        }
        else
        {
           mmal_status_to_int(status);
           vcos_log_error("%s: Failed to connect camera to preview", __func__);
        }
  
  error:
  
        mmal_status_to_int(status);
  
        if (state.common_settings.verbose)
           fprintf(stderr, "Closing down\n");
  
        if (state.useGL)
        {
           raspitex_stop(&state.raspitex_state);
           raspitex_destroy(&state.raspitex_state);
        }
  
        // Disable all our ports that are not handled by connections
        check_disable_port(camera_video_port);
        check_disable_port(encoder_output_port);
  
        if (state.preview_connection)
           mmal_connection_destroy(state.preview_connection);
  
        if (state.encoder_connection)
           mmal_connection_destroy(state.encoder_connection);
  
        /* Disable components */
        if (state.encoder_component)
           mmal_component_disable(state.encoder_component);
  
        if (state.preview_parameters.preview_component)
           mmal_component_disable(state.preview_parameters.preview_component);
  
        if (state.camera_component)
           mmal_component_disable(state.camera_component);
  
        destroy_encoder_component(&state);
        raspipreview_destroy(&state.preview_parameters);
        destroy_camera_component(&state);
  
        if (state.common_settings.verbose)
           fprintf(stderr, "Close down completed, all components disconnected, disabled and destroyed\n\n");
  
        if (state.common_settings.gps)
           raspi_gps_shutdown(state.common_settings.verbose);
     }
  
     if (status != MMAL_SUCCESS)
        raspicamcontrol_check_configuration(128);
  
     return exit_code;
  }
  
  
  