#define _GNU_SOURCE

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <signal.h>
#include <inttypes.h>
#include <unistd.h>
#include <libgen.h>
#include <limits.h>
#include <pthread.h>
#include <ctype.h> // isdigit()
#include <PvApi.h>

#include "libcommon/daemon.h"
#include "libcommon/config.h"
#include "libcommon/getopt.h"
#include "libcommon/lcm_util.h"
#include "libcommon/stdio_util.h"
#include "libcommon/timestamp.h"
#include "libcommon/timeutil.h"

#include "lcmtypes/botlcm_image_t.h"
#include "lcmtypes/botlcm_image_sync_t.h"
#include "lcmtypes/botlcm_image_metadata_t.h"
#include "lcmtypes/senlcm_prosilica_t.h"

#include "prosilica.h"
#include "viewer.h"
#include "burst.h"

#include "/usr/include/opencv/highgui.h"

bool gblDone = false;

typedef struct camera camera_t;
struct camera
{
    unsigned long  uid;          // UniqueId
    tPvHandle      handle;       // Camera handle
    int            nFrames;
    tPvFrame      *Frames;
    int64_t        framecount;
    bool           alive;
};

typedef struct driver driver_t;
struct driver
{
    bool        daemon;     // daemon?

    /* driver */
    int         monitor;      // monitor mode
    char       *attfile;      // prosilica camera attributes file
    int         queue;        // driver image frame buffer depth
    int         multicam;     // auto compute StreamBytesPerSecond for ncameras
    int         burst_mode;   //turn on and off burst mode, default should be off and select a burst mode

 
    /* prosilica */
    int        ConfigFileIndex;         // {'0', '1', '2', '3', '4', '5'}
    char       *ExposureMode;           // {'Manual', 'Auto', 'AutoOnce'}
    uint32_t    ExposureValue;          // microseconds
    double      FrameRate;              // Hz
    char       *FrameStartTriggerMode;  // {'Freerun', 'SyncIn1', 'SyncIn2', 'FixedRate'}
    char       *GainMode;               // {'Manual', 'Auto', 'AutoOnce'}
    uint32_t    GainValue;              // dB
    char       *MulticastEnable;        // {'On', 'Off'}
    char       *PixelFormat;            // {Mono8, Mono16, Bayer8, Bayer16, Rgb24, Rgb48, Yuv411, Yuv422, Yuv444}
    uint32_t    PacketSize;             // Bytes
    uint32_t    StreamBytesPerSecond;   // Bytes per second
    senlcm_prosilica_t pvatt;

    /* logging */
    int         logtodisk;   // log to disk?
    char       *logdir;      // where?
    int         compression; // TIFF compression scheme
    uint8_t     quality;     // 0-100 compression quality, for use with jpeg compression
    char       *strftime;    // image filename format
    int         bayer;       // if color camera, log just the raw bayer pattern w/o interpolation

    /* lcm */
    char       *channel;
    int         publish;     // bool: publish images to lcm ?
    int         SparsePublish;   // int: The interval of images to publish.  That is, we publish 1 in SparsePublish images
    char       *url;         // image lcm url
    bool       view;  //do we want the image viewer on?
    bool       logatt; //logging camera attributes to file in image directory
    FILE      *att_log_file;
};

typedef struct lcminfo lcminfo_t;
struct lcminfo
{
    lcm_t *lcm;
    char  *channel_data;
    char  *channel_sync;
    char  *channel_attributes;
};

typedef struct state state_t;
struct state 
{
    Config      *cfg;
    getopt_t    *gopt;
    camera_t    *camera;
    driver_t    *driver;
    lcminfo_t   *lcminfo;
    pthread_mutex_t * tss_lock;
    pthread_t * tss_thread;
    timestamp_sync_state_t *tss;
    burst_t  *burst;

    char   *rootkey;
    char   *rootdir;
};

typedef struct write_data write_data_t;
struct write_data
{
    tPvFrame *Frame;
    state_t  *state;
    int32_t   framecount;
    int64_t   utime;
};

static void *
attributes_thread (void *context)
{
    state_t *state = context;

    while (1) {
        timeutil_sleep (1);
        if (state->camera->alive) {
            senlcm_prosilica_t *pvatt = prosilica_get_pvattributes (state->camera->handle);
            if (pvatt != NULL) {
                pvatt->self = 1;
		pvatt->start = 0;
		pvatt->stop = 0;
                pvatt->utime = timestamp_now ();
                senlcm_prosilica_t_publish (state->lcminfo->lcm, state->lcminfo->channel_attributes, pvatt);
                
            }
        }
        else
            printf (".");
    }

    return 0;
}


static void *
write_thread (void *context)
{
    write_data_t *wdata = context;
    char timestamp[NAME_MAX];
    char filename[NAME_MAX];
    prosilica_strftime_filename (filename, sizeof filename, wdata->state->driver->strftime, wdata->utime, wdata->framecount);
    
    
    bool color_interpolate = wdata->state->driver->bayer ? false : true;
    prosilica_save_tiff (wdata->Frame, wdata->state->camera->uid, wdata->state->driver->logdir, 
                         filename, color_interpolate, wdata->state->driver->compression, wdata->state->driver->quality);
    //printf("%s\n",filename);
    prosilica_free_frames (wdata->Frame, 1);

    //check the viewing option
    if (wdata->state->driver->view)
      {
	//I have the filename here so this is where i can read it and view it.
	//first pasers the filename into a human readbable timestamp
	
	int parse_timestamp_err =  parse_timestamp( timestamp, 
					     filename, 
					     NAME_MAX );  

	if (parse_timestamp_err)
	  fprintf(stderr,"***parse_timestamp error***\n");


	//concatenate the file name and the logdir so cvLoadImage can grab it
	char im_full_path[255];
	int cat_image_path_err =  cat_image_path( im_full_path,
		    255,
		    wdata->state->driver->logdir,
		    filename );
     
	if (cat_image_path_err)
	  fprintf(stderr,"***cat_image_path error***\n");

     //view the image in a cvNamedWindow
        int view_image_err = view_image( im_full_path, 
				      filename,
				      timestamp );
        if (view_image_err)
          fprintf(stderr,"***view_image error***\n");
      }


    
    //if we want to log image information to a file, we do these next things
    if (wdata->state->driver->logatt)
      {
	 senlcm_prosilica_t *pvatt = prosilica_get_pvattributes (wdata->state->camera->handle);
            if (pvatt != NULL) {
                pvatt->utime = timestamp_now ();
                //here i have to write them to file: att_log_file
		//FIXME: i don't actually want this happening for all attributes, but i need to find otu the indices of the ones i care about
                //for (int i=0; i < pvatt->n_attributes; i++){
		fprintf(wdata->state->driver->att_log_file,"%s\t%s\t%s\n",filename,pvatt->PvAttributes[29].value,pvatt->PvAttributes[33].value); 
                fflush(wdata->state->driver->att_log_file);
		prosilica_free_pvattributes (pvatt);
		}
      }	
    free (wdata);
    return 0;
}

static int32_t
botlcm_pixel_format (tPvFrame *Frame) // mapping from camunits/pixel.h
{
    switch (Frame->Format) {
    case ePvFmtMono8:   return BOTLCM_IMAGE_T_PIXEL_FORMAT_GRAY;
    case ePvFmtMono16:  return BOTLCM_IMAGE_T_PIXEL_FORMAT_LE_GRAY16;
    case ePvFmtBayer8:  
        switch (Frame->BayerPattern) {
        case ePvBayerRGGB: return BOTLCM_IMAGE_T_PIXEL_FORMAT_BAYER_RGGB;
        case ePvBayerGBRG: return BOTLCM_IMAGE_T_PIXEL_FORMAT_BAYER_GBRG;
        case ePvBayerGRBG: return BOTLCM_IMAGE_T_PIXEL_FORMAT_BAYER_GRBG;
        case ePvBayerBGGR: return BOTLCM_IMAGE_T_PIXEL_FORMAT_BAYER_BGGR;
        default: return BOTLCM_IMAGE_T_PIXEL_FORMAT_INVALID;
        }
    case ePvFmtBayer16: return BOTLCM_IMAGE_T_PIXEL_FORMAT_ANY; // unsupported in camunits API
    case ePvFmtRgb24:   return BOTLCM_IMAGE_T_PIXEL_FORMAT_RGB;
    case ePvFmtRgb48:   return BOTLCM_IMAGE_T_PIXEL_FORMAT_BE_RGB16;
    case ePvFmtYuv411:  return BOTLCM_IMAGE_T_PIXEL_FORMAT_IYU1;
    case ePvFmtYuv422:  return BOTLCM_IMAGE_T_PIXEL_FORMAT_UYVY;
    case ePvFmtYuv444:  return BOTLCM_IMAGE_T_PIXEL_FORMAT_IYU2;
    case ePvFmtBgr24:   return BOTLCM_IMAGE_T_PIXEL_FORMAT_BGR;
    case ePvFmtRgba32:  return BOTLCM_IMAGE_T_PIXEL_FORMAT_RGBA;
    case ePvFmtBgra32:  return BOTLCM_IMAGE_T_PIXEL_FORMAT_BGRA;
    default: return BOTLCM_IMAGE_T_PIXEL_FORMAT_INVALID;
    }

    return 3;
}


static void*
ClockSync(void* arg)
{
    state_t *state = (state_t*)arg;
    int64_t TimeNow;
    int64_t ticks;
    int64_t utime;
    int64_t tick_utime;
    for (;;) {
        
        prosilica_compare_timestamps(state->camera->handle, &ticks, &TimeNow);

        printf("\tTimeNow: %llx  ticks: %llx\n", TimeNow, ticks);

        pthread_mutex_lock(state->tss_lock);
        tick_utime = timestamp_lookup(state->tss, ticks);
        utime = timestamp_sync(state->tss, ticks, TimeNow);
        pthread_mutex_unlock(state->tss_lock);

        printf("Syncing... error=%3.6f seconds\n", (tick_utime - TimeNow)/1000000.0);
        printf("\tTPS: %11.9f  hostTime: %llx  devtime: %llx\n", state->tss->dev_ticks_per_second, state->tss->sync_host_time, state->tss->last_dev_ticks);
        
        // give us some time to pause
        sleep(3);

    }   

    return NULL;

}

static void
FrameDoneCB (tPvFrame *Frame)
{

    state_t *state = (state_t *) Frame->Context[0];

    int32_t framecount = state->camera->framecount++;

    switch (Frame->Status) {
    case ePvErrSuccess: {
        int64_t Timestamp = ((int64_t)Frame->TimestampLo) + (((int64_t)Frame->TimestampHi)<<32);

        //recover the current camera-time
        //NOTE: chunks of this should probably move to libcommon/timestamp.c
        //as a new function
        //cameraTimeNow = prosilica_get_timestamp(state->camera->handle);
        //dticks = cameraTimeNow-Timestamp;
        //if (dticks < 0) {
            //dticks += (state->tss->dev_ticks_wraparound);
        //}
        //diff = ((double)(dticks))/(state->tss->dev_ticks_per_second);
        //printf("Diff: %9.6 seconds\n", diff);
        
        
        //timestamp_delta = (int64_t)(diff * 1000000);
        //utime_raw -= timestamp_delta;

        pthread_mutex_lock(state->tss_lock);
        int64_t utime = timestamp_lookup (state->tss, Timestamp);
        pthread_mutex_unlock(state->tss_lock);

        // publish image sync over lcm
        botlcm_image_sync_t sync = { .utime = utime };
        


        botlcm_image_sync_t_publish (state->lcminfo->lcm, state->lcminfo->channel_sync, &sync);


        // clone and requeue the original Frame to the camera
        // This is not strictly necessary, and apprears to be an effort to keep
        // returning frames to the queue as quickly as possible.  As it ain't broke (we think),
        // we won't try to fix it (Ian)
        tPvFrame *dupFrame = prosilica_dup_frames (Frame, 1);
        PvCaptureQueueFrame (state->camera->handle, Frame, FrameDoneCB);

        // bit shift for easy viewing
        if (dupFrame->BitDepth == 12 && (state->driver->publish || state->driver->logtodisk))
            prosilica_bitshift_frame (dupFrame);

        // publish image data over lcm?
        if (state->driver->publish && (state->driver->SparsePublish < 1 || ((Frame->FrameCount) % (state->driver->SparsePublish) == 0))) {
      
            if (state->driver->SparsePublish > 1) {
                printf("\nPUBLISH: %lu\n", Frame->FrameCount);
            }

            int32_t bpp = (dupFrame->BitDepth == 12 ? 2 : 1); // bytes per pixel
            int32_t row_stride;
            if (Frame->Format == ePvFmtMono8  || Frame->Format == ePvFmtBayer8 ||
                Frame->Format == ePvFmtMono16 || Frame->Format == ePvFmtBayer16)
                row_stride = bpp * Frame->Width;
            else
                row_stride = 3 * bpp * Frame->Width; // rgb, yuv

            botlcm_image_t image = {
                .utime  = utime,
                .width  = dupFrame->Width,
                .height = dupFrame->Height,
                .row_stride = row_stride,
                .pixelformat = botlcm_pixel_format (dupFrame),
                .size = dupFrame->ImageBufferSize,
                .data = dupFrame->ImageBuffer,
            };
            botlcm_image_t_publish (state->lcminfo->lcm, state->lcminfo->channel_data, &image);
        }

        // write to disk?
        if (state->driver->logtodisk) {
            write_data_t *wdata = malloc (sizeof (write_data_t));
            wdata->Frame = dupFrame;
            wdata->state = state;
            wdata->framecount = framecount;
            wdata->utime = utime;

            pthread_t tid;
            pthread_create (&tid, NULL, write_thread, wdata);
            pthread_detach (tid); // cut thread loose
            if (state->driver->SparsePublish <= 1) {
                printf ("Frame-drv %d, Frame-cam %d\n", framecount+1, (int)(dupFrame->FrameCount));
            }

        }
        else {
            // frame count
            if (state->driver->SparsePublish <= 1) {
                printf ("Frame-drv %d, Frame-cam %d\n", framecount+1, (int)(dupFrame->FrameCount));
            }

            prosilica_free_frames (dupFrame, 1);
	}	
        
        //here is where we do the bursting if we want
	if (state->driver->burst_mode)
	  {

	    handle_burst(state->burst);
	    //create a pvattstructure like cam attributes does and stuff it with a case statement based on the burst message 
	    //create the structure 
            senlcm_prosilica_t pvatt_burst;
	    pvatt_burst.PvAttributes = (senlcm_prosilica_attribute_t *) calloc (1, sizeof (senlcm_prosilica_attribute_t)); 
            pvatt_burst.n_attributes = 1;
            pvatt_burst.stop = 0;
	    pvatt_burst.start = 0;
	    pvatt_burst.self = 0;
            pvatt_burst.PvAttributes[0].label = "FrameRate";
            switch(state->burst->message)
	      {
	      case DONOTHING:
	         puts("DO NOTHING");
	         break;
	      case SPEEDUP:
	         puts("SPEED UP");
	         pvatt_burst.PvAttributes[0].value = state->burst->fast_rate;   
	         senlcm_prosilica_t_publish (state->lcminfo->lcm, state->lcminfo->channel_attributes, &pvatt_burst);    
	         break;
	     case SLOWDOWN:
	         puts("SLOW DOWN");
	         pvatt_burst.PvAttributes[0].value = state->burst->slow_rate;
                 senlcm_prosilica_t_publish (state->lcminfo->lcm, state->lcminfo->channel_attributes, &pvatt_burst);    
	         break;
	     default:
	         puts("DEFAULT");
	         break;
	     }	    
	    //   prosilica_free_pvattributes (pvatt_burst);
	  }
        break;
    }
    case ePvErrCancelled: {
        break;
    }
    case ePvErrUnplugged: {
        break;
    }
    default:
        PROSILICA_ERROR (Frame->Status, "FrameDoneCB()");
        PvCaptureQueueFrame(state->camera->handle, Frame, FrameDoneCB);
    }
}

static void
PvLinkAdd (state_t *state)
{
    tPvErr err;

    // fail-safe open the camera
    int attempts = 0;
    bool alive = false;
    while (!alive) {
        timeutil_sleep (1);

        if (prosilica_camera_online (state->camera->uid)) {
            if (state->driver->monitor)
                err = PvCameraOpen (state->camera->uid, ePvAccessMonitor, &state->camera->handle);
            else
                err = PvCameraOpen (state->camera->uid, ePvAccessMaster, &state->camera->handle);
            
            if (err == ePvErrSuccess)
                alive = true;
            else {
                if (++attempts > 1 )
                    PROSILICA_ERROR (err, "PvCameraOpen()");
                timeutil_sleep (5);
            }
        }
    }

    // create a timestamp_sync object
    if (state->tss != NULL) {
        printf("Unfreed timestamp encountered on add; possible corruption of time synching\n");
        timestamp_sync_free (state->tss);
    }
    unsigned long TimeStampFrequency;
    PvAttrUint32Get (state->camera->handle, "TimeStampFrequency",  &TimeStampFrequency);
    state->tss = timestamp_sync_init (TimeStampFrequency, LLONG_MAX, 1);
    
    // create a mutex object for the timesync
    state->tss_lock = (pthread_mutex_t*)malloc(sizeof(pthread_mutex_t));
    pthread_mutex_init(state->tss_lock, NULL);


    // configure camera settings
    if (!state->driver->monitor) {
        err = PvCaptureAdjustPacketSize (state->camera->handle, PROSILICA_MAX_PACKET_SIZE);
        if (err != ePvErrSuccess)
            PROSILICA_ERROR (err, "PvCaputerAdjustPacketSize()");

        // restore factory config and set StreamBytesPerSecond to max, otherwise FrameRate and other settings may not take
        prosilica_load_config (state->camera->handle, 0);
        err = PvAttrUint32Set (state->camera->handle, "StreamBytesPerSecond", PROSILICA_STREAMBYTESPERSECOND_MAX);
        if (err != ePvErrSuccess)
            PROSILICA_ERROR (err, "PvAttrUint32Set()");

        // load any custom config settings
        if (state->driver->attfile) {
            if (prosilica_load_attfile (state->camera->handle, state->driver->attfile))
                PROSILICA_ERROR (NULL, "prosilica_load_attfile() failed to load %s", state->driver->attfile);
            else
                printf ("Loaded attfile=%s\n", state->driver->attfile);
        }

        // send over any master.cfg PvAttributes
        prosilica_set_pvattributes (state->camera->handle, &state->driver->pvatt);

        // ensure that command line options, if present, override
        if (getopt_has_flag (state->gopt, "ConfigFileIndex"))
            prosilica_load_config (state->camera->handle, state->driver->ConfigFileIndex);
        if (getopt_has_flag (state->gopt, "ExposureValue"))
            PvAttrUint32Set (state->camera->handle, "ExposureValue", state->driver->ExposureValue);
        if (getopt_has_flag (state->gopt, "ExposureMode"))
            PvAttrEnumSet (state->camera->handle, "ExposureMode", state->driver->ExposureMode);
        if (getopt_has_flag (state->gopt, "FrameRate"))
            PvAttrFloat32Set (state->camera->handle, "FrameRate", state->driver->FrameRate);
        if (getopt_has_flag (state->gopt, "FrameStartTriggerMode"))
            PvAttrEnumSet (state->camera->handle, "FrameStartTriggerMode", state->driver->FrameStartTriggerMode);
        if (getopt_has_flag (state->gopt, "GainValue"))
            PvAttrUint32Set (state->camera->handle, "GainValue", state->driver->GainValue);
        if (getopt_has_flag (state->gopt, "GainMode"))
            PvAttrEnumSet (state->camera->handle, "GainMode", state->driver->GainMode);
        if (getopt_has_flag (state->gopt, "MulticastEnable"))
            PvAttrEnumSet (state->camera->handle, "MulticastEnable", state->driver->MulticastEnable);
        if (getopt_has_flag (state->gopt, "PixelFormat"))
            PvAttrEnumSet (state->camera->handle, "PixelFormat", state->driver->PixelFormat);
        if (getopt_has_flag (state->gopt, "PacketSize"))
            PvAttrUint32Set (state->camera->handle, "PacketSize", state->driver->PacketSize);
        if (getopt_has_flag (state->gopt, "StreamBytesPerSecond"))
            PvAttrUint32Set (state->camera->handle, "StreamBytesPerSecond", state->driver->StreamBytesPerSecond);
  

        // auto StreamBytesPerSecond
        if (getopt_has_flag (state->gopt, "multicam")) {
            long ret = prosilica_multicam (state->camera->handle, state->driver->multicam);
            if (ret < 0)
                PROSILICA_ERROR (NULL, "prosilica_multicam: unable to auto set StreamBytesPerSecond");
        }

        err = PvCaptureStart (state->camera->handle);
        if (err != ePvErrSuccess)
            PROSILICA_ERROR (err, "PvCaptureStart()");
    }

    // alloc frame buffer and queue frames
    state->camera->nFrames = state->driver->queue;
    state->camera->Frames = prosilica_alloc_frames (state->camera->handle, state->camera->nFrames);
    if (state->camera->Frames == NULL) {
        PROSILICA_ERROR (-1, "unable to alloc frame buffer memory - abort");
        abort ();
    }
    for (int i=0; i<state->camera->nFrames; i++) {
        state->camera->Frames[i].Context[0] = state;
        PvCaptureQueueFrame (state->camera->handle, state->camera->Frames+i, FrameDoneCB);
    }

    // user info
    unsigned long TotalBytesPerFrame;
    PvAttrUint32Get (state->camera->handle, "TotalBytesPerFrame", &TotalBytesPerFrame);
    char PixelFormat[256], type[32];
    PvAttrEnumGet (state->camera->handle, "PixelFormat", PixelFormat, sizeof PixelFormat, NULL);
    if (state->driver->monitor)
        sprintf (type, "Monitor");
    else
        sprintf (type, "Master");
    printf ("camera %lu opened and ready: %s, %s, TotalBytesPerFrame=%lu\n", 
            state->camera->uid, type, PixelFormat, TotalBytesPerFrame);

    // bandwidth
    prosilica_bandwidth_t bw;
    int ret = prosilica_bandwidth_used (state->camera->handle, &bw);
    if (ret < 0)
        PROSILICA_ERROR (ret, "prosilica_bandwidth_used()");
    else
        printf ("StreamBytesPerSecond=%ld   Used=%ld   (%.2f%%)\n",
                bw.StreamBytesPerSecond, bw.Used, bw.Percentage);

    // start the clock synchroninzation thread
    state->tss_thread = (pthread_t*)malloc(sizeof(pthread_t));
    pthread_create(state->tss_thread, NULL, ClockSync, state);

    // go forth!
    //PvCommandRun (state->camera->handle, "AcquisitionStart");

    state->camera->alive = true;
}

static void
PvLinkRemove (state_t *state)
{
    state->camera->alive = false;

    if (!state->camera->handle)
        return;

    tPvErr err;
    err = PvCaptureQueueClear (state->camera->handle);
    if (err != ePvErrSuccess)
        PROSILICA_ERROR (err, "PvCaptureQueueClear()");
    prosilica_free_frames (state->camera->Frames, state->camera->nFrames);

    err = PvCaptureEnd (state->camera->handle);
    if (err != ePvErrSuccess)
        PROSILICA_ERROR (err, "PvCameraEnd()");

    err = PvCameraClose (state->camera->handle);
    if (err != ePvErrSuccess)
        PROSILICA_ERROR (err, "PvCameraClose()");
    else
        printf ("camera %lu closed\n", state->camera->uid);
}

static void
LinkEventCB (void* context, tPvInterface interface, tPvLinkEvent event, unsigned long uid)
{
    state_t *state = (state_t *) context;

    if (uid != state->camera->uid)
        return;

    switch(event) {
    case ePvLinkAdd: {
        printf ("camera %lu plugged\n", uid);
        PvLinkAdd (state);
        break;
    }
    case ePvLinkRemove: {
        printf ("camera %lu unplugged\n", uid);
        PvLinkRemove (state);
        printf ("camera %ld waiting\n", state->camera->uid);
        break;
    }
    default:
        break;
    }
}

static void
pvattributes_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                       const senlcm_prosilica_t *pvatt, void *user)
{
     state_t *state = (state_t*) user;
     if (!pvatt->self && !state->driver->monitor && prosilica_camera_online (state->camera->uid)){
       if (pvatt->start){
	   tPvErr err = PvCommandRun (state->camera->handle,"AcquisitionStart");
	   if(state->driver->logatt){

	     fprintf(state->driver->att_log_file,"Filename\tExposureValue\tFrameRate\n");
             fflush(state->driver->att_log_file);
	   }
           if (err != ePvErrSuccess)
	     puts("Acquisition Start was unsuccessful");
	   
       }
       if (pvatt->stop){
	   tPvErr err = PvCommandRun (state->camera->handle,"AcquisitionStop");          
           if (err != ePvErrSuccess)
	     puts("Acquisition Stop was unsuccessful");
       }           
       puts("***Setting Attributes now***");
       prosilica_set_pvattributes (state->camera->handle, pvatt);
     }
}

static state_t *
state_create (int argc, char *argv[])
{
    // init state
    state_t *state = (state_t *) calloc (1, sizeof (state_t));
    state->cfg = config_parse_default ();
    state->gopt = getopt_create ();
    state->camera = (camera_t *) calloc (1, sizeof (camera_t));
    state->driver = (driver_t *) calloc (1, sizeof (driver_t));
    state->lcminfo = (lcminfo_t *) calloc (1, sizeof (lcminfo_t));
    state->burst = (burst_t *) calloc (1, sizeof(burst_t));
    state->rootdir = get_current_dir_name ();
    char rootkey[256];
    sprintf (rootkey, "sensors.%s", basename (argv[0]));
    state->rootkey = strdup (rootkey);
    return state;
}

static void
state_destroy (state_t *state)
{
    if (state->cfg)
        config_free (state->cfg);
    if (state->gopt)
        getopt_destroy (state->gopt);
    if (state->camera)
        free (state->camera);
    if (state->driver)
        free (state->driver);
    if (state->rootkey)
        free (state->rootkey);
    if (state->rootdir)
        free (state->rootdir);
    if (state)
        free (state);
}

static void
my_signal_handler (int signum, siginfo_t *siginfo, void *ucontext_t)
{
    gblDone = true;

    if (signum == SIGTERM) {
        printf("SIGTERM Received! Exiting Normally...\n");
	exit(0);
    }
}


static void
parse_args (state_t *state, int argc, char *argv[])
{
    char key[256];

   /* Add std command-line options */
    getopt_add_description (state->gopt, "Prosilica Gig-E camera driver.");
    getopt_add_help   (state->gopt, NULL);
    getopt_add_bool   (state->gopt, 'D',  "daemon",     0,                          "Run as daemon");
    getopt_add_string (state->gopt, 'k',  "key",        state->rootkey,             "Config file key");

    getopt_add_spacer (state->gopt, "");
    getopt_add_spacer (state->gopt, "Driver Options:");
    getopt_add_long   (state->gopt, 'u',  "uid",         "",                        "UniqueId");
    getopt_add_bool   (state->gopt, 'M',  "monitor",     0,                         "Open camera driver in Monitor instead of Master mode");
    getopt_add_string (state->gopt, 'a',  "attfile",     "",                        "Camera attributes file");
    getopt_add_int    (state->gopt, 'q',  "queue",       "20",                      "Image frame buffer queue depth");
    getopt_add_int    (state->gopt, 'm',  "multicam",    "1",                       "Number of cameras to use in auto StreamBytesPerSecond calc");

    getopt_add_spacer (state->gopt, "");
    getopt_add_spacer (state->gopt, "Prosilica Options:");
    getopt_add_string (state->gopt, '\0', "ConfigFileIndex",       "",              "{'Factory', '1', '2', '3', '4', '5'}");
    getopt_add_string (state->gopt, '\0', "ExposureMode",          "",              "{'Manual', 'Auto', 'AutoOnce'}");
    getopt_add_double (state->gopt, '\0', "ExposureValue",         "",              "usec (valid in manual mode)");
    getopt_add_double (state->gopt, '\0', "FrameRate",             "",              "valid in FixedRate mode");
    getopt_add_string (state->gopt, '\0', "FrameStartTriggerMode", "",              "{'Freerun', 'SyncIn1', 'SyncIn2', 'FixedRate'}");
    getopt_add_string (state->gopt, '\0', "GainMode",              "",              "{'Manual', 'Auto', 'AutoOnce'}");
    getopt_add_double (state->gopt, '\0', "GainValue",             "",              "dB (valid in manual gain mode)");
    getopt_add_bool   (state->gopt, '\0', "MulticastEnable",       0,               "Multicast stream instead of unicasting it");
    getopt_add_string (state->gopt, '\0', "PixelFormat",           "",              "{Mono8, Mono16, Bayer8, Bayer16, Rgb24, Rgb48, Yuv411, Yuv422, Yuv444}");
    getopt_add_int    (state->gopt, '\0', "PacketSize",            "",              "Bytes");
    getopt_add_long   (state->gopt, '\0', "StreamBytesPerSecond",  "",              "Bytes per second");

    getopt_add_spacer (state->gopt, "");
    getopt_add_spacer (state->gopt, "Logging Options:");
    getopt_add_string (state->gopt, 'd',  "logdir",      "./",                      "Image directory");
    getopt_add_bool   (state->gopt, 'l',  "logtodisk",   0,                         "Log .tif images to disk");
    getopt_add_string (state->gopt, '\0', "compression", "none",                    ".tif compression scheme {none,packbits,lzw,jpeg,jpeg:xx,deflate}");
    getopt_add_string (state->gopt, '\0', "strftime",    "IMG-%s.%i-%f.tif",        "Image file name strftime format, %f for frame number, %i for usecs");
    getopt_add_bool   (state->gopt, '\0', "bayer",       0,                         "Log raw Bayer pattern");
    getopt_add_spacer (state->gopt, "");
    getopt_add_spacer (state->gopt, "LCM:");
    getopt_add_string (state->gopt, 'c',  "channel",     "PROSILICA",               "LCM channel, e.g., SENSOR_FOO");
    getopt_add_bool   (state->gopt, 'p',  "publish",     0,                         "LCM publish image_t data");
    getopt_add_int   (state->gopt, 's',  "SparsePublish", "1",                   "Only publish 1 in SparsePublish images; used for high-speed capture.  Limited to 1-100 (inclusive).  To publish all images, set to 1");
    getopt_add_string (state->gopt, '\0', "url",         "",                        "LCM publish url (default is NULL)");
    getopt_add_bool (state->gopt, 'v', "view",         0,                        "Default is false, do you want to use the viewer?  don't use with the laser");
    getopt_add_int (state->gopt, '\0', "burstmode",    "0",      "Burst mode, default is off");
    getopt_add_bool (state->gopt, '\0',"logatt", 0, "Attribute logging to the image directory");

    if (!getopt_parse (state->gopt, argc, argv, 1) || state->gopt->extraargs->len!=0) {
        getopt_do_usage (state->gopt, NULL);
        exit (EXIT_FAILURE);
    }
    else if (getopt_get_bool (state->gopt, "help")) {
        getopt_do_usage (state->gopt, NULL);
        exit (EXIT_SUCCESS);
    }

    // daemon
    state->driver->daemon = getopt_get_bool (state->gopt, "daemon");

    // key
    state->rootkey = strdup (getopt_get_string (state->gopt, "key"));

    /* DRIVER OPTS */

    // uid
    sprintf (key, "%s.uid", state->rootkey);
    if (getopt_has_flag (state->gopt, "uid"))
        state->camera->uid = getopt_get_long (state->gopt, "uid");
    else if (config_get_long (state->cfg, key, (long *) &state->camera->uid) != 0) {
        ERROR ("unspecified camera uid");
        exit (EXIT_FAILURE);
    }

    // monitor
    sprintf (key, "%s.monitor", state->rootkey);
    if (getopt_has_flag (state->gopt, "monitor"))
        state->driver->monitor = getopt_get_bool (state->gopt, "monitor");
    else if (config_get_boolean (state->cfg, key, &state->driver->monitor) != 0)
        state->driver->monitor = getopt_get_bool (state->gopt, "monitor"); /* default */

    // attfile
    sprintf (key, "%s.attfile", state->rootkey);
    if (getopt_has_flag (state->gopt, "attfile"))
        state->driver->attfile = strdup (getopt_get_string (state->gopt, "attfile"));
    else if (config_get_string (state->cfg, key, &state->driver->attfile) != 0)
        state->driver->attfile = strdup (getopt_get_string (state->gopt, "attfile"));
    if (strlen (state->driver->attfile) == 0) {
        free (state->driver->attfile);
        state->driver->attfile = NULL;
    }
    
    // queue
    sprintf (key, "%s.queue", state->rootkey);
    if (getopt_has_flag (state->gopt, "queue"))
        state->driver->queue = getopt_get_int (state->gopt, "queue");
    else if (config_get_int (state->cfg, key, &state->driver->queue) != 0)
        state->driver->queue = getopt_get_int (state->gopt, "queue"); /* default */

    // multicam
    sprintf (key, "%s.multicam", state->rootkey);
    if (getopt_has_flag (state->gopt, "multicam"))
        state->driver->multicam = getopt_get_int (state->gopt, "multicam");
    else if (config_get_int (state->cfg, key, &state->driver->multicam) != 0)
        state->driver->multicam = getopt_get_int (state->gopt, "multicam"); /* default */

    /* PROSILICA OPTS */

    // ConfigFileIndex
    if (getopt_has_flag (state->gopt, "ConfigFileIndex")) {
        int index = 0;
        const char *index_str = getopt_get_string (state->gopt, "ConfigFileIndex");
        if (!strcasecmp (index_str, "Factory"))
            state->driver->ConfigFileIndex = 0;
        else if (isdigit (index_str[0]) && (index=atoi (index_str)) && 0<index && index <= 5)
            state->driver->ConfigFileIndex = index;
        else {
            PROSILICA_ERROR (NULL, "unrecognized argument to --ConfigFileIndex");
            exit (EXIT_FAILURE);
        }
    }

    // ExposureValue
    if (getopt_has_flag (state->gopt, "ExposureValue"))
        state->driver->ExposureValue = getopt_get_int (state->gopt, "ExposureValue");

    // ExposureMode
    if (getopt_has_flag (state->gopt, "ExposureMode")) {
        const char *expmode = getopt_get_string (state->gopt, "ExposureMode");
        if (!strcasecmp (expmode, "Manual"))
            state->driver->ExposureMode = strdup ("Manual");
        else if (!strcasecmp (expmode, "Auto"))
            state->driver->ExposureMode = strdup ("Auto");
        else if (!strcasecmp (expmode, "AutoOnce"))
            state->driver->ExposureMode = strdup ("AutoOnce");
        else {
            PROSILICA_ERROR (NULL, "unrecognized argument to --ExposureMode");
            exit (EXIT_FAILURE);
        }
    }

    // FrameRate
    if (getopt_has_flag (state->gopt, "FrameRate"))
        state->driver->FrameRate = getopt_get_double (state->gopt, "FrameRate");

    // FrameStartTriggerMode
    if (getopt_has_flag (state->gopt, "FrameStartTriggerMode")) {
        const char *trigger = getopt_get_string (state->gopt, "FrameStartTriggerMode");
        if (!strcasecmp (trigger, "Freerun"))
            state->driver->FrameStartTriggerMode = strdup ("Freerun");
        else if (!strcasecmp (trigger, "SyncIn1"))
            state->driver->FrameStartTriggerMode = strdup ("SyncIn1");
        else if (!strcasecmp (trigger, "SyncIn2"))
            state->driver->FrameStartTriggerMode = strdup ("SyncIn2");
        else if (!strcasecmp (trigger, "FixedRate"))
            state->driver->FrameStartTriggerMode = strdup ("FixedRate");
        else {
            PROSILICA_ERROR (NULL, "unrecognized argument to --FrameStartTriggerMode");
            exit (EXIT_FAILURE);
        }
    }

    // MulticastEnable
    if (getopt_get_bool (state->gopt, "MulticastEnable"))
        state->driver->MulticastEnable = strdup ("On");
    else
        state->driver->MulticastEnable = strdup ("Off");


    // GainValue
    if (getopt_has_flag (state->gopt, "GainValue"))
        state->driver->GainValue = getopt_get_int (state->gopt, "GainValue");

    // GainMode
    if (getopt_has_flag (state->gopt, "GainMode")) {
        const char *gainmode = getopt_get_string (state->gopt, "GainMode");
        if (!strcasecmp (gainmode, "Manual"))
            state->driver->GainMode = strdup ("Manual");
        else if (!strcasecmp (gainmode, "Auto"))
            state->driver->GainMode = strdup ("Auto");
        else if (!strcasecmp (gainmode, "AutoOnce"))
            state->driver->GainMode = strdup ("AutoOnce");
        else {
            PROSILICA_ERROR (NULL, "unrecognized argument to --GainMode");
            exit (EXIT_FAILURE);
        }
    }

    // PixelFormat
    if (getopt_has_flag (state->gopt, "PixelFormat")) {
        const char *pixelformat = getopt_get_string (state->gopt, "PixelFormat");
        if (!strcasecmp (pixelformat, "Mono8"))
            state->driver->PixelFormat = strdup ("Mono8");
        else if (!strcasecmp (pixelformat, "Mono16"))
            state->driver->PixelFormat = strdup ("Mono16");
        else if (!strcasecmp (pixelformat, "Bayer8"))
            state->driver->PixelFormat = strdup ("Bayer8");
        else if (!strcasecmp (pixelformat, "Bayer16"))
            state->driver->PixelFormat = strdup ("Bayer16");
        else if (!strcasecmp (pixelformat, "Rgb24"))
            state->driver->PixelFormat = strdup ("Rgb24");
        else if (!strcasecmp (pixelformat, "Rgb48"))
            state->driver->PixelFormat = strdup ("Rgb48");
        else if (!strcasecmp (pixelformat, "Yuv411"))
            state->driver->PixelFormat = strdup ("Yuv411");
        else if (!strcasecmp (pixelformat, "Yuv422"))
            state->driver->PixelFormat = strdup ("Yuv422");
        else if (!strcasecmp (pixelformat, "Yuv444"))
            state->driver->PixelFormat = strdup ("Yuv444");
        else if (!strcasecmp (pixelformat, "Bgr24"))
            state->driver->PixelFormat = strdup ("Bgr24");
        else if (!strcasecmp (pixelformat, "Rgba32"))
            state->driver->PixelFormat = strdup ("Rgba32");
        else if (!strcasecmp (pixelformat, "Bgra32"))
            state->driver->PixelFormat = strdup ("Bgra32");
        else {
            PROSILICA_ERROR (NULL, "unrecognized argument to --PixelFormat");
            exit (EXIT_FAILURE);
        }
    }

    // PacketSize
    if (getopt_has_flag (state->gopt, "PacketSize"))
        state->driver->PacketSize = getopt_get_int (state->gopt, "PacketSize");

    // StreamBytesPerSecond
    if (getopt_has_flag (state->gopt, "StreamBytesPerSecond"))
        state->driver->StreamBytesPerSecond = getopt_get_long (state->gopt, "StreamBytesPerSecond");

    // PvAttributes
    sprintf (key, "%s.PvAttributes", state->rootkey);
    int nkeys = config_get_num_subkeys (state->cfg, key);
    char **subkeys = config_get_subkeys (state->cfg, key);
    state->driver->pvatt.n_attributes = nkeys;
    state->driver->pvatt.PvAttributes = (senlcm_prosilica_attribute_t *) calloc (nkeys, sizeof (senlcm_prosilica_attribute_t));
    for (int i=0; i<nkeys; i++) {
        char *subkey = subkeys[i];
        state->driver->pvatt.PvAttributes[i].label = subkey;
        char subkeyfull[256];
        sprintf (subkeyfull, "%s.%s", key, subkey);
        config_get_string (state->cfg, subkeyfull, &state->driver->pvatt.PvAttributes[i].value);
    }


    /* LOGGING OPTS */

    // logtodisk
    sprintf (key, "%s.logtodisk", state->rootkey);
    if (getopt_has_flag (state->gopt, "logtodisk"))
        state->driver->logtodisk = getopt_get_bool (state->gopt, "logtodisk");
    else if (config_get_boolean (state->cfg, key, &state->driver->logtodisk) != 0)
        state->driver->logtodisk = getopt_get_bool (state->gopt, "logtodisk"); /* default */

    // logdir
    sprintf (key, "%s.logdir", state->rootkey);
    if (getopt_has_flag (state->gopt, "logdir"))
        state->driver->logdir = strdup (getopt_get_string (state->gopt, "logdir"));
    else if (config_get_string (state->cfg, key, &state->driver->logdir) != 0)
        state->driver->logdir = strdup (getopt_get_string (state->gopt, "logdir")); /* default */
   
    //logatt
    state->driver->logatt = getopt_get_bool (state->gopt, "logatt");

    //viewer
    if (getopt_has_flag (state->gopt, "view"))
        state->driver->view = getopt_get_bool (state->gopt, "view");
    else  
      state->driver->view = 0;
        

    
    // compression
    sprintf (key, "%s.compression", state->rootkey);
    char *compression;
    if (getopt_has_flag (state->gopt, "compression"))
        compression = (char *) getopt_get_string (state->gopt, "compression");
    else if (config_get_string (state->cfg, key, &compression) != 0)
        compression = (char *) getopt_get_string (state->gopt, "compression"); /* default */

    state->driver->quality = 95;
    if (!strcasecmp (compression, "none"))
        state->driver->compression = COMPRESSION_NONE;
    else if (!strcasecmp (compression, "jpeg")) 
        state->driver->compression = COMPRESSION_JPEG;
    else if (!strncasecmp (compression, "jpeg:",5)) {
        state->driver->compression = COMPRESSION_JPEG;
        unsigned int q;
        if (1==sscanf (compression, "jpeg:%u", &q))
            state->driver->quality = q;
        else
            PROSILICA_ERROR (NULL, "jpeg quality not parseable");
    }
    else if (!strcasecmp (compression, "packbits"))
        state->driver->compression = COMPRESSION_PACKBITS;
    else if (!strcasecmp (compression, "lzw"))
        state->driver->compression = COMPRESSION_LZW;
    else if (!strcasecmp (compression, "deflate"))
        state->driver->compression = COMPRESSION_DEFLATE;
    else {
        PROSILICA_ERROR (NULL, "unrecognized argument to --compression");
        exit (EXIT_FAILURE);
    }
    

    // strftime
    sprintf (key, "%s.strftime", state->rootkey);
    if (getopt_has_flag (state->gopt, "strftime"))
        state->driver->strftime = strdup (getopt_get_string (state->gopt, "strftime"));
    else if (config_get_string (state->cfg, key, &state->driver->strftime) != 0)
        state->driver->strftime = strdup (getopt_get_string (state->gopt, "strftime"));

    // bayer
    sprintf (key, "%s.bayer", state->rootkey);
    if (getopt_has_flag (state->gopt, "bayer"))
        state->driver->bayer = getopt_get_bool (state->gopt, "bayer");
    else if (config_get_boolean (state->cfg, key, &state->driver->bayer) != 0)
        state->driver->bayer = getopt_get_bool (state->gopt, "bayer"); /* default */


    /* LCM OPTS */

    // channel
    sprintf (key, "%s.channel", state->rootkey);
    if (getopt_has_flag (state->gopt, "channel"))
        state->driver->channel = strdup (getopt_get_string (state->gopt, "channel"));
    else if (config_get_string (state->cfg, key, &state->driver->channel) != 0)
        state->driver->channel = strdup (getopt_get_string (state->gopt, "channel")); /* default */

    char channel[LCM_MAX_CHANNEL_NAME_LENGTH];
    snprintf (channel, LCM_MAX_CHANNEL_NAME_LENGTH, "%s", state->driver->channel);
    state->lcminfo->channel_data = strdup (channel);

    snprintf (channel, LCM_MAX_CHANNEL_NAME_LENGTH, "%s.%s", state->driver->channel, "SYNC");
    state->lcminfo->channel_sync = strdup (channel);

    snprintf (channel, LCM_MAX_CHANNEL_NAME_LENGTH, "%s.%s", state->driver->channel, "ATTRIBUTES");
    state->lcminfo->channel_attributes = strdup (channel);


    // publish
    sprintf (key, "%s.publish", state->rootkey);
    if (getopt_has_flag (state->gopt, "publish"))
        state->driver->publish = getopt_get_bool (state->gopt, "publish");
    else if (config_get_boolean (state->cfg, key, &state->driver->publish) != 0)
        state->driver->publish = getopt_get_bool (state->gopt, "publish"); /* default */

    // Sparse Publish
    sprintf (key, "%s.SparsePublish", state->rootkey);
    if (getopt_has_flag (state->gopt, "SparsePublish")) {
        state->driver->SparsePublish = getopt_get_int (state->gopt, "SparsePublish");
        if (state->driver->SparsePublish > 100)
            state->driver->SparsePublish = 100;
        if (state->driver->SparsePublish < 1)
            state->driver->SparsePublish = 1;
    }

    // url
    sprintf (key, "%s.url", state->rootkey);
    if (getopt_has_flag (state->gopt, "url"))
        state->driver->url = strdup (getopt_get_string (state->gopt, "url"));
    else if (config_get_string (state->cfg, key, &state->driver->url) != 0)
        state->driver->url = strdup (getopt_get_string (state->gopt, "url")); /* default */

    if (strlen (state->driver->url) == 0) {
        free (state->driver->url);
        state->driver->url = NULL;
    }
    // burst mode
    state->driver->burst_mode = getopt_get_int (state->gopt, "burstmode");
    printf("\nBurst mode is %d\n", state->driver->burst_mode);
    init_burst(state->driver->burst_mode,state->burst);
    printf("fast frame rate is %s\n",state->burst->fast_rate);
}


int main (int argc, char *argv[])
{
    // so that redirected stdout won't be insanely buffered.
    setvbuf (stdout, (char *) NULL, _IONBF, 0);

    state_t *state = state_create (argc, argv);
    parse_args (state, argc, argv);

    if (getopt_get_bool (state->gopt, "daemon")) {
        daemon_fork ();
        close (STDERR_FILENO);
    }

    // make sure logging directory exists
    if (state->driver->logtodisk && stdiou_mkpath (state->driver->logdir, 0775) < 0) {
        PERROR ("stdiou_mkpath()");
        exit (EXIT_FAILURE);
    }
    //make a file to log attributes to
   if (state->driver->logatt){
        char attlog_fullpath[255];
   	int cat_image_path_err =  cat_image_path(attlog_fullpath,
		    255,
		    state->driver->logdir,
		    LOGATTFILE );
 	if (cat_image_path_err)
	  fprintf(stderr,"***cat_image_path error***\n");
	else
	  printf("Creating %s for attribute logging\n",attlog_fullpath);
	 state->driver->att_log_file = fopen(attlog_fullpath,"w+");
        
       }
   else
     printf("**we arne't going to make a file for attribute logging\n");


    // bring up the API
    tPvErr err = PvInitialize ();
    if (err != ePvErrSuccess) {
        PROSILICA_ERROR (err, "PvInitialize()");
        exit (EXIT_FAILURE);
    }
    
    printf ("camera %ld waiting\n", state->camera->uid);

    // register a callback for when cameras enter/leave the bus
    PvLinkCallbackRegister (LinkEventCB, ePvLinkAdd, state);
    PvLinkCallbackRegister (LinkEventCB, ePvLinkRemove, state);


    // fire up lcm
    state->lcminfo->lcm = lcm_create (NULL);
    if (!state->lcminfo->lcm) {
        ERROR ("lcm_create() failed");
        exit (EXIT_FAILURE);
    }
    senlcm_prosilica_t_subscribe (state->lcminfo->lcm, state->lcminfo->channel_attributes,
                                  &pvattributes_callback, state);

    // install custom signal handler
    struct sigaction act = {
        .sa_sigaction = my_signal_handler,
    };
    sigfillset (&act.sa_mask);
    act.sa_flags |= SA_SIGINFO;
    sigaction (SIGTERM, &act, NULL);
    sigaction (SIGINT,  &act, NULL);


    // launch attributes thread
    pthread_t tid;
    pthread_create (&tid, NULL, attributes_thread, state);
    pthread_detach (tid);

    while (!gblDone)
        lcm_handle (state->lcminfo->lcm);

    // free resources
    lcm_destroy (state->lcminfo->lcm);
    PvLinkRemove (state);
    PvUnInitialize ();
    printf ("%s: Goodbye\n", state->rootkey);
    //FIXME: also if( fopen was sucessful)
    if  (state->driver->logatt){
       fclose(state->driver->att_log_file);
       puts("closing attribute logfile");
       }
}
