#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/prosilica.h"

typedef struct driver driver_t;
struct driver
{
 /* prosilica */
    senlcm_prosilica_t pvatt_cam;
    senlcm_prosilica_t pvatt_new;
 
    bool      get;
    bool      set;
    bool      start;
    bool      stop;
 
};

typedef struct lcminfo lcminfo_t;
struct lcminfo
{
  lcm_t *lcm;
  char  *channel_attributes;
  senlcm_prosilica_t_subscription_t *attribute_subscription;
};

typedef struct state my_state_t;
struct state
{
  getopt_t  *gopt;
  driver_t  *driver;
  lcminfo_t *lcminfo;
};

static my_state_t *
state_create ( void )
{
    // init state
    my_state_t *state = (my_state_t *) calloc (1, sizeof (my_state_t));
    state->gopt = getopt_create ();
    state->driver = (driver_t *) calloc (1, sizeof (driver_t));
    state->lcminfo = (lcminfo_t *) calloc (1, sizeof (lcminfo_t));
    state->lcminfo->channel_attributes = "PROSILICA_C.ATTRIBUTES";
    
    
    return state;
}

//parse the arguments, 
//determine how many keys there are,
//init the pvatt structure
static void
parse_args (my_state_t *state, int argc, char *argv[])
{
    int nkeys = 0;

    /* Add std command-line options*/
    getopt_add_description  (state->gopt,  "Prosilica Options");
    getopt_add_help         (state->gopt,  NULL);
    
    getopt_add_bool (state->gopt, 'O',  "Start",                 0, "Start Acquisition");
    
    getopt_add_bool (state->gopt, 'X', "Stop",                   0, "Stop Acquisition");
    
    getopt_add_bool (state->gopt, 'g',  "get",                   0, "Get Attributes");
    getopt_add_bool (state->gopt, 's',  "set",                   0, "Set Attributes");
    
    getopt_add_bool (state->gopt, 'c',  "color",                 0,             "Specify color cam");
    getopt_add_bool (state->gopt, 'm',  "mono",                  0,             "Specify mono cam");

    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_int    (state->gopt, '\0', "BinningX",              "",              "Binning in the X direction");
    getopt_add_int    (state->gopt, '\0', "BinningY",              "",              "Binning in the Y direction");
    getopt_add_int    (state->gopt, '\0', "Height",                "",              "Height of the image to take (max is 1024)");
    getopt_add_int    (state->gopt, '\0', "Width",                 "",              "Width of the image to take (max is 1380)");
    getopt_add_int    (state->gopt, '\0', "RegionX",               "",              "The X position of the top-left corner of the ROI, in pixels");
    getopt_add_int    (state->gopt, '\0', "RegionY",               "",              "The Y position of the top-left corner of the ROI, in pixels");



    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);
    }

        /*Setting the options*/
    
      state->driver->set = getopt_get_bool(state->gopt,"set");
      state->driver->get = getopt_get_bool(state->gopt,"get");

      state->driver->start = getopt_get_bool(state->gopt,"Start");
      state->driver->stop = getopt_get_bool(state->gopt,"Stop");

    //Key
    if (!getopt_has_flag (state->gopt, "color") && !getopt_has_flag (state->gopt, "mono"))
    {
      puts("You must specify a camera");
      exit (EXIT_FAILURE);
    }


    // ExposureValue
    if (getopt_has_flag (state->gopt, "ExposureValue"))
	nkeys++;

    // ExposureMode
    if (getopt_has_flag (state->gopt, "ExposureMode"))
        nkeys++;


    // FrameRate
    if (getopt_has_flag (state->gopt, "FrameRate"))
        nkeys++;

    // FrameStartTriggerMode
    if (getopt_has_flag (state->gopt, "FrameStartTriggerMode")) 
        nkeys++;

    // MulticastEnable
    if (getopt_get_bool (state->gopt, "MulticastEnable"))
        nkeys++; 

    // GainValue
    if (getopt_has_flag (state->gopt, "GainValue"))
        nkeys++;

    // GainMode
    if (getopt_has_flag (state->gopt, "GainMode")) 
        nkeys++;

    // PixelFormat
    if (getopt_has_flag (state->gopt, "PixelFormat")) 
         nkeys++;

    // PacketSize
    if (getopt_has_flag (state->gopt, "PacketSize"))
        nkeys++;

    // StreamBytesPerSecond
    if (getopt_has_flag (state->gopt, "StreamBytesPerSecond"))
        nkeys++;

    // BinningX
    if (getopt_has_flag (state->gopt, "BinningX"))
        nkeys++;

    // BinningY
    if (getopt_has_flag (state->gopt, "BinningY"))
        nkeys++;

    // Width
    if (getopt_has_flag (state->gopt, "Width"))
        nkeys++;

    // Height
    if (getopt_has_flag (state->gopt, "Height"))
        nkeys++;

    // RegionX
    if (getopt_has_flag (state->gopt, "RegionX"))
        nkeys++;

    // RegionY
    if (getopt_has_flag (state->gopt, "RegionY"))
        nkeys++;


    //init the new pvatt structure
    state->driver->pvatt_new.PvAttributes = (senlcm_prosilica_attribute_t *) calloc (nkeys, sizeof (senlcm_prosilica_attribute_t));
    if (state->driver->get)
        state->driver->pvatt_cam.PvAttributes = (senlcm_prosilica_attribute_t *) calloc (nkeys, sizeof (senlcm_prosilica_attribute_t));
    state->driver->pvatt_new.n_attributes = nkeys;
    state->driver->pvatt_new.self = 0;
   
    if (state->driver->start && !state->driver->stop){
        state->driver->pvatt_new.start = 1;
        state->driver->pvatt_new.stop = 0;
        puts("Starting Aquisition");
    }
    else if (state->driver->stop && !state->driver->start){
        state->driver->pvatt_new.start = 0;
        state->driver->pvatt_new.stop = 1;
        puts("Stopping Aquisition");
    }
    else{
        state->driver->pvatt_new.start = 0;
        state->driver->pvatt_new.stop = 0;
    }

}

//fill the pvatt structure with the attribute keys
static void
fill_pvatt (my_state_t *state)
{

  int i = 0    ;
    /*Setting the options*/
        
    //key
    if (getopt_has_flag (state->gopt, "color"))
    {
        state->lcminfo->channel_attributes = "PROSILICA_C.ATTRIBUTES";   
    }
    else if (getopt_has_flag (state->gopt, "mono"))
    {
        state->lcminfo->channel_attributes = "PROSILICA_M.ATTRIBUTES";
    }
    else
    {
        puts("Quitting: somehow your camera specification didn't take");
        exit (EXIT_FAILURE);
    }

     // ExposureValue
    if (getopt_has_flag (state->gopt, "ExposureValue"))
    {
        state->driver->pvatt_new.PvAttributes[i].value = 
	            strdup(getopt_get_string (state->gopt, "ExposureValue"));
        state->driver->pvatt_new.PvAttributes[i].label = 
                    strdup("ExposureValue");
        i++;
    }

    // ExposureMode
    if (getopt_has_flag (state->gopt, "ExposureMode"))
    {
        const char *expmode = getopt_get_string (state->gopt, "ExposureMode");
	state->driver->pvatt_new.PvAttributes[i].label = strdup("ExposureMode");

        if (!strcasecmp (expmode, "Manual"))
            state->driver->pvatt_new.PvAttributes[i].value = strdup ("Manual");
        else if (!strcasecmp (expmode, "Auto"))
            state->driver->pvatt_new.PvAttributes[i].value = strdup ("Auto");
        else if (!strcasecmp (expmode, "AutoOnce"))
            state->driver->pvatt_new.PvAttributes[i].value = strdup ("AutoOnce");
        else {
            PROSILICA_ERROR (NULL, "unrecognized argument to --ExposureMode");
            exit (EXIT_FAILURE);
        }
	i++;
    }

    // FrameRate
    if (getopt_has_flag (state->gopt, "FrameRate"))
    {
        state->driver->pvatt_new.PvAttributes[i].label = 
	      strdup("FrameRate");
        state->driver->pvatt_new.PvAttributes[i].value = 
	  strdup(getopt_get_string (state->gopt, "FrameRate"));
	i++;
    }

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

    // GainValue
    if (getopt_has_flag (state->gopt, "GainValue"))
    {
        state->driver->pvatt_new.PvAttributes[i].label = strdup ("GainValue");
        state->driver->pvatt_new.PvAttributes[i].value = strdup (getopt_get_string (state->gopt, "GainValue"));
	i++;
    }


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

    // PacketSize
    if (getopt_has_flag (state->gopt, "PacketSize"))
    {
          state->driver->pvatt_new.PvAttributes[i].label = strdup("PacketSize");
	  state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "PacketSize"));
	  i++;
    }

    // StreamBytesPerSecond
    if (getopt_has_flag (state->gopt, "StreamBytesPerSecond"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("SteamBytesPerSecond");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "StreamBytesPerSecond"));
         i++;
    }

    // Binning X
    if (getopt_has_flag (state->gopt, "BinningX"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("BinningX");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "BinningX"));
         i++;
    }

    // Binning Y
    if (getopt_has_flag (state->gopt, "BinningY"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("BinningY");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "BinningY"));
         i++;
    }

    // Height
    if (getopt_has_flag (state->gopt, "Height"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("Height");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "Height"));
         i++;
    }

    // Width
    if (getopt_has_flag (state->gopt, "Width"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("Width");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "Width"));
         i++;
    }

    // Region X
    if (getopt_has_flag (state->gopt, "RegionX"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("RegionX");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "RegionX"));
         i++;
    }

    // Region Y
    if (getopt_has_flag (state->gopt, "RegionY"))
    {
         state->driver->pvatt_new.PvAttributes[i].label = strdup("RegionY");
         state->driver->pvatt_new.PvAttributes[i].value = strdup(getopt_get_string (state->gopt, "RegionY"));
         i++;
    }

}

senlcm_prosilica_t 
get_wanted_attributes( senlcm_prosilica_t pvatt_short, senlcm_prosilica_t pvatt_long )
{

   //init a pvatt structure that will contain 
   //the requested information from the camera
   senlcm_prosilica_t pvatt_cam;
   pvatt_cam.PvAttributes = (senlcm_prosilica_attribute_t *) calloc (pvatt_short.n_attributes, sizeof (senlcm_prosilica_attribute_t));
   pvatt_cam.n_attributes = pvatt_short.n_attributes;
   pvatt_cam.self = 1;
   pvatt_cam.utime = pvatt_long.utime;
   pvatt_cam.start = pvatt_long.start;
   pvatt_cam.stop = pvatt_long.stop;


  puts("Current Prosilica Settings");
  puts("--------------------------");

  for (int i=0; i<pvatt_short.n_attributes; i++)
  {
     for (int j=0; j<pvatt_long.n_attributes; j++)
     {
       if (!strcmp(pvatt_short.PvAttributes[i].label,
		   pvatt_long.PvAttributes[j].label))
       {
	  pvatt_cam.PvAttributes[i].label = 
	        strdup(pvatt_long.PvAttributes[j].label);
	  pvatt_cam.PvAttributes[i].value = 
	        strdup(pvatt_long.PvAttributes[j].value);
          printf("%s:%s\n",
		pvatt_cam.PvAttributes[i].label,
		pvatt_cam.PvAttributes[i].value);
       }
     }
  }
  return pvatt_cam;
}
   

static void
pvattributes_callback (const lcm_recv_buf_t *rbuf, const char *channel, 
                       const senlcm_prosilica_t *pvatt, void *user)
{
    my_state_t *state = (my_state_t*) user;
    //if the pvatt struct came from the cameras and not from this program
    //read it
    if (pvatt->self)
      {  
  
      senlcm_prosilica_t_unsubscribe (state->lcminfo->lcm, 
				      state->lcminfo->attribute_subscription);
      
      //extract and print attributes
      state->driver->pvatt_cam = get_wanted_attributes(state->driver->pvatt_new,
						       *pvatt);
      }
}

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

    my_state_t *state = state_create ();
    parse_args (state, argc, argv);
    
    //create lcm
    state->lcminfo->lcm = lcm_create (NULL);
    
    //move the arguments into an lcm pvatt stucture
    fill_pvatt (state);

    //if we have specified that we want to get attributes, 
    //subscribe and print them
    if (state->driver->get)
    {
        state->lcminfo->attribute_subscription = senlcm_prosilica_t_subscribe (
				      state->lcminfo->lcm, 
				      state->lcminfo->channel_attributes,
				      &pvattributes_callback, 
				      state);
        while(!state->driver->pvatt_cam.PvAttributes[0].label) {
	   puts("Waiting for Godot");
	   lcm_handle(state->lcminfo->lcm);

	}
    }     
   //if we have decided to set attributes
    //publish and print them
    if (state->driver->set)
    {
      //print the attributes if there are attributes in the modification table
      if (state->driver->pvatt_new.n_attributes > 0){
	 puts("Camera Attribute Modifications");
	 puts("------------------------------");
         for (int i=0; i<state->driver->pvatt_new.n_attributes; i++){
            printf("%s: %s\n",state->driver->pvatt_new.PvAttributes[i].label,
                   state->driver->pvatt_new.PvAttributes[i].value);
         }
      }
      else
	puts("No attributes are being modified");
 
      //publish no matter what just in case we are just starting and stopping
      senlcm_prosilica_t_publish (state->lcminfo->lcm, state->lcminfo->channel_attributes, &state->driver->pvatt_new);

     }

     lcm_destroy(state->lcminfo->lcm);

}
