/*************************************************************************************/
/*
* File name: PacketDelay.cpp
* Location: See Matrox Example Launcher in the MIL Control Center
 * 
*
* Synopsis:  This program shows how to calculate inter-packet delays for your GigE
*            Vision� camera. The resulting table printed at the end of this example
*            can be used to program the appropriate delay in user applications using
*            MdigControl with M_GC_INTER_PACKET_DELAY.
*
*      Note: The inter-packet delay is initially set to zero. A reference frame rate
*            is then sampled and used for subsequent calculations. The algorithm then
*            proceeds and calculates a theoretical inter-packet delay for the current 
*            camera parameters (SizeX, SizeY, PacketSize, PixelFormat). This theoretical
*            delay is used as a starting point. At this point acquisition is re-started
*            and the obtained frame-rate is compared to the reference frame-rate.
*            Modifications to the theoretical inter-packet delay are then performed when
*            needed. This process repeats iteratively until the obtained frame rate
*            converges to the reference frame rate. If the reference frame rate initially
*            sampled is off, then the algorithm will not converge to the solution.
*/
#include <mil.h>
#if M_MIL_USE_WINDOWS
#include <conio.h>
#include <windows.h>
#endif

/* Number of images in the buffering grab queue.
Generally, increasing this number gives better real-time grab.
*/
#define BUFFERING_SIZE_MAX 20

/* Set this define to 1 to print additional details performed by this example. */
#define PRINT_DETAILS      0

/* User's processing function prototype. */
MIL_INT MFTYPE ProcessingFunction(MIL_INT HookType,
                                  MIL_ID HookId,
                                  void* HookDataPtr);

/* Struct and variable definitions/declarations. */
MIL_ID MilGrabBufferList[BUFFERING_SIZE_MAX] = { 0 };
MIL_INT MilGrabBufferListSize;

typedef struct
   {
   MIL_DOUBLE BaseFrameRate;
   MIL_DOUBLE ProcessFrameRate;
   MIL_DOUBLE DelayInSeconds;
   MIL_UINT64 TickFreq;
   MIL_INT DelayTickVal;
   MIL_INT ProcessFrameCount;
   MIL_INT EqualityCounter;
   bool Error;
   }PacketDelayInfo;

typedef struct _PacketDelayResults
   {
   ~_PacketDelayResults()
      {
      for(MIL_INT i=0; i<PixelFormatCount; i++)
         delete [] PixelFormats[i];

      delete [] PixelFormats;
      delete [] InterPacketDelayInTicks;
      delete [] InterPacketDelayInSec;
      delete [] ReferenceFrameRate;
      delete [] ObtainedFrameRate;
      }

   MIL_INT PixelFormatCount;
   MIL_TEXT_PTR* PixelFormats;
   MIL_INT* InterPacketDelayInTicks;
   MIL_DOUBLE* InterPacketDelayInSec;
   MIL_DOUBLE* ReferenceFrameRate;
   MIL_DOUBLE* ObtainedFrameRate;
   long Selection;
   }PacketDelayResults;

/* Utility functions. */
void EnumeratePixelFormats(MIL_ID MilDigitizer, MIL_INT BoardType, PacketDelayResults& Results);
void ApplyPixelFormat(MIL_ID MilDigitizer, PacketDelayResults& Results);
void AllocateAcquisitionBuffers(MIL_ID MilSystem, MIL_ID MilDigitizer, MIL_INT BoardType);
void AcquireReferenceFrameRate(MIL_ID MilDigitizer, PacketDelayInfo& Info, PacketDelayResults& Results);
void FindInterPacketDelay(MIL_ID MilDigitizer, PacketDelayInfo& Info, PacketDelayResults& Results);
void PrintResults(MIL_ID MilDigitizer, PacketDelayInfo& Info, PacketDelayResults& Results);
void GetMilBufferInfoFromPixelFormat(MIL_ID MilDigitizer, MIL_INT& SizeBand,
                                     MIL_INT& BufType, MIL_INT64& Attribute);
bool IsEqual(MIL_DOUBLE A, MIL_DOUBLE B)
   {
   if(((A+0.1) >= B) && ((A-0.1) <= B))
      return true;
   else
      return false;
   }

/* Main function. */
/* -------------- */

int MosMain(void)
   {
   MIL_ID MilApplication;
   MIL_ID MilSystem     ;
   MIL_ID MilDigitizer  ;
   MIL_INT BoardType = 0;
   MIL_INT NbIterations = 0, i = 0;
   PacketDelayInfo PktInfo;
   PacketDelayResults Results;
   memset(&PktInfo, 0, sizeof(PacketDelayInfo));
   memset(&Results, 0, sizeof(PacketDelayResults));

   /* Allocate defaults. */
   MappAllocDefault(M_DEFAULT, &MilApplication, &MilSystem, M_NULL,
      &MilDigitizer, M_NULL);
   
   /* Inquire board type. */
   MsysInquire(MilSystem, M_BOARD_TYPE, &BoardType);

   /* This example only runs on gige vision systems. */
   if((BoardType != M_GIGE_VISION))
      {
      MosPrintf(MIL_TEXT("This example only runs on GigE Vision systems.\n"));   
      MappFreeDefault(MilApplication, MilSystem, M_NULL, MilDigitizer, M_NULL);
      return 0;
      }

   /* Inquire the camera's clock tick frequency. */
   MdigInquire(MilDigitizer, M_GC_COUNTER_TICK_FREQUENCY, &PktInfo.TickFreq);
   if(PktInfo.TickFreq == 0)
      {
      MosPrintf(MIL_TEXT("Error, camera does not support inter-packet delay.\n"));
      
      /* Release defaults. */
      MappFreeDefault(MilApplication, MilSystem, M_NULL, MilDigitizer, M_NULL);
      return 0;
      }

   /* Print a message. */
   MosPrintf(MIL_TEXT("\nThis example shows how to calculate inter-packet\n"));
   MosPrintf(MIL_TEXT("delay for your GigE Vision camera.\n\n"));
   MosPrintf(MIL_TEXT("Inter-packet delay is used to spread packet transmission\n"));
   MosPrintf(MIL_TEXT("over the length of a frame. This is done to minimize the chance\n"));
   MosPrintf(MIL_TEXT("of FIFO overruns inside your Gigabit Ethernet controller.\n"));
   MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n\n"));
   MosGetch();

   /* Print the camera's pixel formats and wait for user selections. */
   EnumeratePixelFormats(MilDigitizer, BoardType, Results);
   if(Results.Selection == Results.PixelFormatCount)
      {
      NbIterations = Results.PixelFormatCount;
      Results.Selection = 0;
      }
   else
      NbIterations = 1;
   
   /* Iterate through the user's selected pixel formats. */
   while(NbIterations--)
      {
      memset(&PktInfo, 0, sizeof(PacketDelayInfo));
      
      /* Inquire the camera's clock frequency so we can convert clock ticks to seconds. */
      MdigInquire(MilDigitizer, M_GC_COUNTER_TICK_FREQUENCY, &PktInfo.TickFreq);

      /* Apply the next pixel format for calculation. */
      ApplyPixelFormat(MilDigitizer, Results);
      
      /* Allocate grab buffers matching the camera's pixel format. */
      AllocateAcquisitionBuffers(MilSystem, MilDigitizer, BoardType);

      /* Print a message. */
      MosPrintf(MIL_TEXT("\n\nCalculating inter-packet delay for %s.\n\n"), Results.PixelFormats[Results.Selection]);
      
      /* Get the reference frame rate. */
      AcquireReferenceFrameRate(MilDigitizer, PktInfo, Results);

      /* With the reference frame rate found, find the optimal inter-packet delay. */
      FindInterPacketDelay(MilDigitizer, PktInfo, Results);

      /* Free the grab buffers. */
      while(MilGrabBufferListSize > 0)
         MbufFree(MilGrabBufferList[--MilGrabBufferListSize]);
      
      Results.Selection++;
      }

   /* Print results. */
   PrintResults(MilDigitizer, PktInfo, Results);
   MosPrintf(MIL_TEXT("Press <Enter> to quit.\n\n\n"));
   MosGetch();
   
   /* Reset inter-packet delay to zero. */
   MdigControl(MilDigitizer, M_GC_INTER_PACKET_DELAY, 0);

   /* Release defaults. */
   MappFreeDefault(MilApplication, MilSystem, M_NULL, MilDigitizer, M_NULL);

   return 0;
   }

/* Enumerate the camera's pixel formats. Only MIL compatible formats are printed. */
/* ------------------------------------------------------------------------------ */
void EnumeratePixelFormats(MIL_ID MilDigitizer, MIL_INT BoardType, PacketDelayResults& Results)
   {
   long SpreadFactor = 0;
   MIL_INT64 PixFmt = 0;
   Results.Selection = -1;
   MIL_INT PixelFormats = 0;
   
   /* Inquire the number of pixel formats supported by the camera. */
   MdigInquireFeature(MilDigitizer, M_FEATURE_ENUM_ENTRY_COUNT, MIL_TEXT("PixelFormat"), M_DEFAULT, &Results.PixelFormatCount);
   if(Results.PixelFormatCount)
      {
      bool Done = false;
      MIL_INT Len = 0;
      Results.PixelFormats = new MIL_TEXT_PTR[Results.PixelFormatCount];
      Results.InterPacketDelayInTicks = new MIL_INT[Results.PixelFormatCount];
      Results.InterPacketDelayInSec = new MIL_DOUBLE[Results.PixelFormatCount];
      Results.ReferenceFrameRate = new MIL_DOUBLE[Results.PixelFormatCount];
      Results.ObtainedFrameRate = new MIL_DOUBLE[Results.PixelFormatCount];

      MosPrintf(MIL_TEXT("Your camera supports the following pixel formats:\n"));
      for(MIL_INT i=0; i<Results.PixelFormatCount; i++)
         {
         Results.InterPacketDelayInTicks[PixelFormats] = 0;
         Results.InterPacketDelayInSec[PixelFormats] = 0;
         Results.ReferenceFrameRate[PixelFormats] = 0;
         Results.ObtainedFrameRate[PixelFormats] = 0;

         /* Get the nth pixel format's string length. . */
         MdigInquireFeature(MilDigitizer, M_FEATURE_ENUM_ENTRY_NAME+M_STRING_SIZE+i, MIL_TEXT("PixelFormat"), M_DEFAULT, &Len);
         Results.PixelFormats[PixelFormats] = new MIL_TEXT_CHAR[Len];
         
         /* Get the nth pixel format's name and numerical value. */
         MdigInquireFeature(MilDigitizer, M_FEATURE_ENUM_ENTRY_NAME+i, MIL_TEXT("PixelFormat"), M_DEFAULT, Results.PixelFormats[PixelFormats]);
         MdigInquireFeature(MilDigitizer, M_FEATURE_ENUM_ENTRY_VALUE+i, MIL_TEXT("PixelFormat"), M_TYPE_ENUMERATION, &PixFmt);

         /* Validate that the pixel format is compatible with MIL. */
         if(PixFmt & PFNC_CUSTOM)
            {
            /* Custom pixel formats are not supported; skip. */
            delete [] Results.PixelFormats[PixelFormats];
            }
         else
            {
            MIL_INT SizeBand = 0, BufType = 0;
            MIL_INT64 Attribute = 0;
            GetMilBufferInfoFromPixelFormat(MilDigitizer, SizeBand, BufType, Attribute);
            if(Attribute)
               {
               MosPrintf(MIL_TEXT("%d %s\n"), PixelFormats, Results.PixelFormats[PixelFormats]);
               PixelFormats++;
               }
            else
               delete [] Results.PixelFormats[PixelFormats];
            }
         }

      Results.PixelFormatCount = PixelFormats;
      /* Add an entry so the user can perform calculations on All pixel formats. */
      if(Results.PixelFormatCount > 1)
         MosPrintf(MIL_TEXT("%d All\n"), Results.PixelFormatCount);
      
      /* Wait for user selection. */
      if(Results.PixelFormatCount > 1)
         {
         do
            {
            MosPrintf(MIL_TEXT("\nPlease select the pixel format that you want use for inter-packet delay\n"));
            MosPrintf(MIL_TEXT("calculation (0-%d): "), Results.PixelFormatCount);
#if M_MIL_USE_WINDOWS
            scanf_s("%d-", &(Results.Selection));
#else
            scanf("%ld-", &(Results.Selection));
#endif
            if(Results.Selection >= 0 && Results.Selection <= Results.PixelFormatCount)
               Done = true;
            else
               MosPrintf(MIL_TEXT("Invalid selection, please try again\n."));
            }
         while(!Done);

         MosPrintf(MIL_TEXT("\n%s selected\n\n"),
            Results.Selection < Results.PixelFormatCount ? Results.PixelFormats[Results.Selection] : MIL_TEXT("All"));
         }
      else
         Results.Selection = 0;
      }
   }

/* Set the camera's pixel format to the next value. */
/* ------------------------------------------------ */
void ApplyPixelFormat(MIL_ID MilDigitizer, PacketDelayResults& Results)
   {
   MdigControlFeature(MilDigitizer, M_FEATURE_VALUE_AS_STRING, MIL_TEXT("PixelFormat"), M_DEFAULT,
      Results.PixelFormats[Results.Selection]);
   }

/* Allocate acquisition buffers compatible with the camera's pixel format. */
/* ----------------------------------------------------------------------- */
void AllocateAcquisitionBuffers(MIL_ID MilSystem, MIL_ID MilDigitizer, MIL_INT BoardType)
   {
   MIL_INT SizeBand = 1;
   MIL_INT BufType = 8+M_UNSIGNED;
   MIL_INT64 AdditionalAttributes = 0;

   /* On the M_GIGE_VISION system, turn off the pixel-format switching feature. */
   /* Also turn off the automatic Bayer conversion feature. */
   /* We must also allocate grab buffers that are of the same format as the camera. */
   if(BoardType == M_GIGE_VISION)
      {
      MdigControl(MilDigitizer, M_GC_PIXEL_FORMAT_SWITCHING, M_DISABLE);
      MdigControl(MilDigitizer, M_BAYER_CONVERSION, M_DISABLE);
      GetMilBufferInfoFromPixelFormat(MilDigitizer, SizeBand, BufType, AdditionalAttributes);
      }

   /* Allocate the grab buffers and clear them. */
   MappControl(M_DEFAULT, M_ERROR, M_PRINT_DISABLE);
   for(MilGrabBufferListSize = 0; 
      MilGrabBufferListSize<BUFFERING_SIZE_MAX; MilGrabBufferListSize++)
      {
      MbufAllocColor(MilSystem,
         SizeBand,
         MdigInquire(MilDigitizer, M_SIZE_X, M_NULL),
         MdigInquire(MilDigitizer, M_SIZE_Y, M_NULL),
         BufType,
         M_IMAGE+M_GRAB+M_PROC+AdditionalAttributes,
         &MilGrabBufferList[MilGrabBufferListSize]);

      if (MilGrabBufferList[MilGrabBufferListSize])
         {
         MbufClear(MilGrabBufferList[MilGrabBufferListSize], 0xFF);
         }
      else
         break;
      }
   MappControl(M_DEFAULT, M_ERROR, M_PRINT_ENABLE);
   }

/* Use MdigProcess to acquire a reference frame rate with the inter-packet delay to zero. */
/* -------------------------------------------------------------------------------------- */
void AcquireReferenceFrameRate(MIL_ID MilDigitizer, PacketDelayInfo& Info, PacketDelayResults& Results)
   {
   /* Set initial inter-packet delay to zero; this is to measure the base frame rate of the
      camera. */
   MdigControl(MilDigitizer, M_GC_INTER_PACKET_DELAY, 0);

   /* Start the MdigProcess; here we want to record a base frame rate that
      will be used for our calculations later. */
   MdigProcess(MilDigitizer, MilGrabBufferList, MilGrabBufferListSize,
      M_SEQUENCE+M_COUNT(BUFFERING_SIZE_MAX), M_DEFAULT, ProcessingFunction, M_NULL);

   MdigProcess(MilDigitizer, MilGrabBufferList, MilGrabBufferListSize,
      M_STOP, M_DEFAULT, ProcessingFunction, M_NULL);

   /* Inquire the reference frame rate. */
   MdigInquire(MilDigitizer, M_PROCESS_FRAME_RATE, &Info.BaseFrameRate);
   Results.ReferenceFrameRate[Results.Selection] = Info.BaseFrameRate;

   /* With the frame-rate estimated, inquire the theoretical inter-packet delay to use. */
   MdigInquire(MilDigitizer, M_GC_THEORETICAL_INTER_PACKET_DELAY, &Info.DelayInSeconds);

   /* Convert the delay from seconds to camera ticks. */
   Info.DelayTickVal = (MIL_UINT32)(Info.DelayInSeconds * Info.TickFreq);
   }

/* Iteratively find a solution that maximizes the inter-packet delay without      */
/* disturbing the frame-rate of the camera.                                       */
/* ------------------------------------------------------------------------------ */
void FindInterPacketDelay(MIL_ID MilDigitizer, PacketDelayInfo& Info, PacketDelayResults& Results)
   {
   bool Done = false;

#if PRINT_DETAILS
   MosPrintf(MIL_TEXT("Reference frame-rate used: %.2f\n\n"), Info.BaseFrameRate);
#endif

   while(!Done)
      {
      /* Set the delay in the camera. Initially this delay corresponds to the value
         returned by MdigInquire with M_GC_THEORETICAL_INTER_PACKET_DELAY. */
      MdigControl(MilDigitizer, M_GC_INTER_PACKET_DELAY, Info.DelayTickVal);

      /* Start acquisition. */
      MdigProcess(MilDigitizer, MilGrabBufferList, MilGrabBufferListSize,
         M_SEQUENCE+M_COUNT(BUFFERING_SIZE_MAX), M_DEFAULT, ProcessingFunction, M_NULL);

      /* Inquire the obtained frame rate with the current inter-packet delay. */
      MdigInquire(MilDigitizer, M_PROCESS_FRAME_RATE,  &Info.ProcessFrameRate);

      /* Stop acquisition. */
      MdigProcess(MilDigitizer, MilGrabBufferList, MilGrabBufferListSize,
         M_STOP, M_DEFAULT, ProcessingFunction, M_NULL);

#if PRINT_DETAILS
      MosPrintf(MIL_TEXT("%Programming delay of %d ticks; frame-rate obtained: %.2f\r"),
         Info.DelayTickVal, Info.ProcessFrameRate);
#else
      MosPrintf(MIL_TEXT("."));
#endif

      /* Validate if obtained frame rate matches reference frame rate. If it does not,
         reduce the inter packet delay and try another iteration. */
      if(IsEqual(Info.BaseFrameRate, Info.ProcessFrameRate))
         {
         /* Frame rate inquired is equal to the base frame rate; we are converging on
            the solution. */
         Info.EqualityCounter++;

         if(Info.DelayTickVal == 0)
            {
            Info.DelayInSeconds = 0.0;
            Info.Error = true;
            Done = true;
            }
         else if(Info.EqualityCounter == 3)
            {
            Done = true;
            /* Found optimal solution, remove an additional 15%. */
            Info.DelayInSeconds -= (Info.DelayInSeconds * 15.0 / 100.0);
            if(Info.DelayInSeconds <= 0.0)
               Info.DelayInSeconds = 0.0;

            Info.DelayTickVal = (MIL_UINT32)(Info.DelayInSeconds * Info.TickFreq);
            MdigControl(MilDigitizer, M_GC_INTER_PACKET_DELAY, Info.DelayTickVal);
            }
         else
            {
            /* Reduce delay for next iteration. */
            Info.DelayInSeconds -= (Info.DelayInSeconds / 50.0);
            Info.DelayTickVal = (MIL_UINT32)(Info.DelayInSeconds * Info.TickFreq);
            }
         }
      else
         {
         /* We are still far from the reference frame rate, reduce delay for next
            iteration. */
         Info.EqualityCounter = 0;
         Info.DelayInSeconds -= (Info.DelayInSeconds / 10.0);
         Info.DelayTickVal = (MIL_UINT32)(Info.DelayInSeconds * Info.TickFreq);
         if(Info.DelayTickVal == 0)
            {
            Info.DelayInSeconds = 0.0;
            Info.Error = true;
            Done = true;
            }
         else if(Info.DelayInSeconds <= 0.0)
            {
            Info.DelayInSeconds = 0.0;
            Done = true;
            }
         }
      
      MosSleep(500);
      }

   /* Store solution in Results struct. This will get printed at the end of the example. */
   if(Info.Error == false)
      {
      Results.InterPacketDelayInTicks[Results.Selection] = Info.DelayTickVal;
      Results.InterPacketDelayInSec[Results.Selection] = Info.DelayInSeconds;
      Results.ObtainedFrameRate[Results.Selection] = Info.ProcessFrameRate;
      }
   }

/* Print the results for each pixel format. */
/* ---------------------------------------- */
void PrintResults(MIL_ID MilDigitizer, PacketDelayInfo& Info, PacketDelayResults& Results)
   {
   MIL_TEXT_PTR lpModel, lpVendor;
   MIL_INT lModelStringLength, lVendorStringLength;
   MIL_INT PacketSize = 0;
   MIL_TEXT_CHAR PixelFormat[255] = {'\0'};
   memset(PixelFormat, 0, sizeof(MIL_TEXT_CHAR)*255);

   /* Found optimal solution, print results. */
   MdigInquire(MilDigitizer, M_CAMERA_VENDOR_SIZE, &lVendorStringLength);
   MdigInquire(MilDigitizer, M_CAMERA_MODEL_SIZE, &lModelStringLength);
   lpVendor = new MIL_TEXT_CHAR[lVendorStringLength+1];
   lpModel = new MIL_TEXT_CHAR[lModelStringLength+1];

   MdigInquire(MilDigitizer, M_CAMERA_VENDOR, lpVendor);
   MdigInquire(MilDigitizer, M_CAMERA_MODEL, lpModel);
   MdigInquire(MilDigitizer, M_GC_PACKET_SIZE, &PacketSize);

#if M_MIL_USE_WINDOWS
   system("cls");
#endif
   MosPrintf(MIL_TEXT("Inter-packet delay report summary for %s %s:\n\n"), lpVendor, lpModel);
   MosPrintf(MIL_TEXT("Camera parameters:\n"));
   MosPrintf(MIL_TEXT("Camera SizeX:         %d\n"), MdigInquire(MilDigitizer, M_SIZE_X, M_NULL));
   MosPrintf(MIL_TEXT("Camera SizeY:         %d\n"), MdigInquire(MilDigitizer, M_SIZE_Y, M_NULL));
   MosPrintf(MIL_TEXT("Camera Packet size:   %d\n\n"), PacketSize);

   for(MIL_INT i=0; i<Results.PixelFormatCount; i++)
      {
      MosPrintf(MIL_TEXT("Camera Pixel format:  %s\n"), Results.PixelFormats[i]);
      MosPrintf(MIL_TEXT("Inter-packet delay of %d ticks (%.3f usec) calculated.\n"),
         Results.InterPacketDelayInTicks[i], Results.InterPacketDelayInSec[i]*1e6);
      MosPrintf(MIL_TEXT("Reference frame rate: %.1f\n"), Results.ReferenceFrameRate[i]);
      MosPrintf(MIL_TEXT("Obtained frame rate:  %.1f\n"), Results.ObtainedFrameRate[i]);
      MosPrintf(MIL_TEXT("----------------------------------------------------------\n"));
      }

   MosPrintf(MIL_TEXT("\nPrinted inter-packet delay results are valid only for ")
      MIL_TEXT("the above parameters\n"));
   
   delete [] lpVendor;
   delete [] lpModel;
   }

/* User's processing function called every time a grab buffer is modified. */
/* ----------------------------------------------------------------------- */
MIL_INT MFTYPE ProcessingFunction(MIL_INT HookType,
                                  MIL_ID HookId,
                                  void* HookDataPtr)
   {
   MIL_ID ModifiedBufferId;

   /* Retrieve the MIL_ID of the grabbed buffer. */
   MdigGetHookInfo(HookId, M_MODIFIED_BUFFER+M_BUFFER_ID, &ModifiedBufferId);

   return 0;
   }
 
/* Get the MIL buffer attributes that matches the camera's pixel format. */
/* --------------------------------------------------------------------- */
void GetMilBufferInfoFromPixelFormat(MIL_ID MilDigitizer, MIL_INT& SizeBand,
                                     MIL_INT& BufType, MIL_INT64& Attribute)
   {
   MdigInquire(MilDigitizer, M_SIZE_BAND, &SizeBand);
   MdigInquire(MilDigitizer, M_TYPE, &BufType);
   MdigInquire(MilDigitizer, M_SOURCE_DATA_FORMAT, &Attribute);
   }