/*************************************************************************************/
/*
* File name:   ProcessingFpga.cpp
* Location: See Matrox Example Launcher in the MIL Control Center
 * 
*
* Synopsis:    This program shows how to use the Radient Processing FPGA.
*
*              This example uses the standard Radient processing FPGA firmware
*              and can accelerate the following MIL functions:
*              1- MbufBayer
*              2- MimArithMultiple (M_OFFSET_GAIN)
*              3- MimLutMap
*
*      Note:   Correct buffer allocation is critical for processing to be
*              offloaded by the processing FPGA. All source buffers must
*              allocated in M_FPGA_ACCESSIBLE memory.
*
* Copyright (C) Matrox Electronic Systems Ltd., 1992-2020.
* All Rights Reserved
*/

#include <mil.h>
#include <math.h>

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

/* For bayer processing, you must choose the bayer filter type for your camera. */
#define BAYER_PATTERN         M_BAYER_RG
/*#define BAYER_PATTERN         M_BAYER_BG */
/*#define BAYER_PATTERN         M_BAYER_GB */
/*#define BAYER_PATTERN         M_BAYER_GR */

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

typedef enum { eBayerProcessing, eOffsetGainProcessing, eLutMapProcessing,
               eUnknownProcessing} eProcessingType;

/* User's processing function hook data structure. */
typedef struct
   {
   MIL_ID  MilImageDisp;
   MIL_ID  WhiteBalance;
   MIL_ID  MilGainImage;
   MIL_ID  MilOffsetImage;
   MIL_ID  MilLutBuffer;
   eProcessingType ProcessingToDo;
   MIL_INT BitShift;
   MIL_INT ProcessedImageCount;
   } HookDataStruct;

void AllocateProcessingBuffers(MIL_ID MilSystem, MIL_ID MilDigitizer, MIL_ID MilDisplay,
                               MIL_INT SizeBand, MIL_INT SizeX, MIL_INT SizeY,
                               MIL_INT SizeBit, HookDataStruct* Struct);

/* Main function. */
/* ---------------*/
int MosMain(void)
   {
   MIL_ID  MilApplication;
   MIL_ID  MilSystem     ;
   MIL_ID  MilDigitizer  ;
   MIL_ID  MilDisplay    ;
   MIL_ID  MilGrabBufferList[BUFFERING_SIZE_MAX] = { 0 };
   MIL_INT MilGrabBufferListSize;
   MIL_INT ProcessFrameCount  = 0;
   MIL_INT UserSelection = 0;
   double  ProcessFrameRate   = 0;
   HookDataStruct UserHookData;
   MIL_INT SizeX, SizeY, SizeBand, SizeBit;

   /* Allocate defaults. */
   MappAllocDefault(M_DEFAULT, &MilApplication, &MilSystem, &MilDisplay, 
                    &MilDigitizer, M_NULL);

   if(MsysInquire(MilSystem, M_PROCESSOR_NUM, M_NULL) == 0)
      {
      MosPrintf(MIL_TEXT("Error, this example demonstrates processing FPGA usage\n"));
      MosPrintf(MIL_TEXT("No processing FPGA detected.\n"));
      MosPrintf(MIL_TEXT("Press <Enter> to quit.\n"));
      MosGetch();
      MappFreeDefault(MilApplication, MilSystem, MilDisplay, MilDigitizer,
         M_NULL);
      return(1);
      }

   MosPrintf(MIL_TEXT("This example demonstrates how the use the Radient Processing")
             MIL_TEXT("Fpga\n"));
   MosPrintf(MIL_TEXT("Please select the desired processing operation to perform.\n"));
   MosPrintf(MIL_TEXT("(1)- MbufBayer\n"));
   MosPrintf(MIL_TEXT("(2)- MimArithMultiple (M_OFFSET_GAIN)\n"));
   MosPrintf(MIL_TEXT("(3)- MimLutMap\n"));
   UserSelection = MosGetch();

   switch(UserSelection)
      {
      case '1':
         MosPrintf(MIL_TEXT("\nMbufBayer selected.\n\n"));
         UserHookData.ProcessingToDo = eBayerProcessing;
         break;
      case '2':
         MosPrintf(MIL_TEXT("\nMimArithMultiple selected.\n\n"));
         UserHookData.ProcessingToDo = eOffsetGainProcessing;
         break;
      case '3':
         MosPrintf(MIL_TEXT("\nMimLutMap selected.\n\n"));
         UserHookData.ProcessingToDo = eLutMapProcessing;
         break;
      default:
         MosPrintf(MIL_TEXT("\nInvalid selection !.\n\nDefaulting to MimLutMap .\n\n"));
         UserHookData.ProcessingToDo = eLutMapProcessing;
         break;
      }

   SizeX = MdigInquire(MilDigitizer, M_SIZE_X, M_NULL);
   SizeY = MdigInquire(MilDigitizer, M_SIZE_Y, M_NULL);
   SizeBand = MdigInquire(MilDigitizer, M_SIZE_BAND, M_NULL);
   SizeBit = MdigInquire(MilDigitizer, M_SIZE_BIT, M_NULL);

   /* Allocate the grab buffers and clear them. */
   MappControl(M_DEFAULT, M_ERROR, M_PRINT_DISABLE);

   AllocateProcessingBuffers(MilSystem, MilDigitizer, MilDisplay, SizeBand, SizeX, SizeY,
                             SizeBit, &UserHookData);

   MappControl(M_DEFAULT, M_ERROR, M_PRINT_ENABLE);


   /* Print a message. */
   MosPrintf(MIL_TEXT("\nMULTIPLE BUFFERED PROCESSING.\n"));
   MosPrintf(MIL_TEXT("-----------------------------\n\n"));
   MosPrintf(MIL_TEXT("Press <Enter> to start.\n\n"));

   /* Grab continuously on the display and wait for a key press. */
   MdigGrabContinuous(MilDigitizer, UserHookData.MilImageDisp);
   MosGetch();

   /* Halt continuous grab. */
   MdigHalt(MilDigitizer);

   if(UserHookData.ProcessingToDo == eBayerProcessing)
      {
      MIL_ID MilTempBuffer;
      /* For bayer processing, we must perform a white balance operation,
      White balancing is not supported by the Processing Fpga, MIL will
      therefore perform this operation using the host CPU instead. */

      /* Allocate a temporary host buffer */
      MbufAlloc2d(MilSystem,
                  SizeX,
                  SizeY,
                  ((SizeBit == 8) ? 8+M_UNSIGNED : 16+M_UNSIGNED),
                  M_IMAGE+M_GRAB,
                  &MilTempBuffer);

      MdigGrab(MilDigitizer, MilTempBuffer);
      MbufBayer(MilTempBuffer, UserHookData.MilImageDisp, UserHookData.WhiteBalance,
                BAYER_PATTERN+M_WHITE_BALANCE_CALCULATE);

      MbufFree(MilTempBuffer);
      }

   /* 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,
         ((UserHookData.ProcessingToDo == eBayerProcessing) ? 1 : SizeBand),
         SizeX,
         SizeY,
         ((SizeBit == 8) ? 8 + M_UNSIGNED : 16 + M_UNSIGNED),
         M_IMAGE + M_GRAB + M_PROC + M_FPGA_ACCESSIBLE,
         &MilGrabBufferList[MilGrabBufferListSize]);

      if (MilGrabBufferList[MilGrabBufferListSize])
         {
         MbufClear(MilGrabBufferList[MilGrabBufferListSize], 0xFF);
         MbufControl(MilGrabBufferList[MilGrabBufferListSize], M_MAX,
            pow(2.0, (MIL_DOUBLE)SizeBit) - 1);
         }
      else
         break;
      }
   MappControl(M_DEFAULT, M_ERROR, M_PRINT_ENABLE);

   /* Initialize the User's processing function data structure. */
   UserHookData.ProcessedImageCount = 0;

   /* Start the processing. The processing function is called for every frame grabbed. */
   MdigProcess(MilDigitizer, MilGrabBufferList, MilGrabBufferListSize,
               M_START, M_DEFAULT, ProcessingFunction, &UserHookData);


   /* NOTE: Now the main() is free to perform other tasks while the processing is */
   /* executing. */
   /* --------------------------------------------------------------------------- */


   /* Print a message and wait for a key press after a minimum number of frames. */
   MosPrintf(MIL_TEXT("Press <Enter> to stop.\n\n"));
   MosGetch();

   /* Stop the processing. */
   MdigProcess(MilDigitizer, MilGrabBufferList, MilGrabBufferListSize,
               M_STOP+M_WAIT, M_DEFAULT, ProcessingFunction, &UserHookData);


   /* Print statistics. */
   MdigInquire(MilDigitizer, M_PROCESS_FRAME_COUNT,  &ProcessFrameCount);
   MdigInquire(MilDigitizer, M_PROCESS_FRAME_RATE,   &ProcessFrameRate);
   MosPrintf(MIL_TEXT("\n\n%lld frames grabbed at %.1f frames/sec (%.1f ms/frame).\n"),
             (long long)ProcessFrameCount, ProcessFrameRate, 1000.0/ProcessFrameRate);
   MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
   MosGetch();

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

   if(UserHookData.MilGainImage)
      MbufFree(UserHookData.MilGainImage);
   if(UserHookData.MilOffsetImage)
      MbufFree(UserHookData.MilOffsetImage);
   if(UserHookData.WhiteBalance)
      MbufFree(UserHookData.WhiteBalance);
   if(UserHookData.MilLutBuffer)
      MbufFree(UserHookData.MilLutBuffer);

   /* Release defaults. */
   MappFreeDefault(MilApplication, MilSystem, MilDisplay, MilDigitizer,
                   UserHookData.MilImageDisp);

   return 0;
   }


/* User's processing function called every time a grab buffer is modified. */
/* ----------------------------------------------------------------------- */

MIL_INT MFTYPE ProcessingFunction(MIL_INT HookType,
                                  MIL_ID HookId,
                                  void* HookDataPtr)
   {
   HookDataStruct *UserHookDataPtr = (HookDataStruct *)HookDataPtr;
   MIL_ID ModifiedBufferId;

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

   /* Print and draw the frame count. */
   UserHookDataPtr->ProcessedImageCount++;
   MosPrintf(MIL_TEXT("Processing frame #%lld.\r"), (long long)UserHookDataPtr->ProcessedImageCount);

   /* Perform the processing and update the display. */

   if(UserHookDataPtr->ProcessingToDo == eBayerProcessing)
      {
      MbufBayer(ModifiedBufferId, UserHookDataPtr->MilImageDisp,
                UserHookDataPtr->WhiteBalance, 
                BAYER_PATTERN+M_BAYER_BIT_SHIFT(UserHookDataPtr->BitShift));
      }
   else if(UserHookDataPtr->ProcessingToDo == eOffsetGainProcessing)
      {
      MimArithMultiple(ModifiedBufferId, UserHookDataPtr->MilOffsetImage,
                       UserHookDataPtr->MilGainImage, 8, M_NULL,
                       UserHookDataPtr->MilImageDisp, M_OFFSET_GAIN, M_DEFAULT);
      }
   else if(UserHookDataPtr->ProcessingToDo == eLutMapProcessing)
      {
      MimLutMap(ModifiedBufferId, UserHookDataPtr->MilImageDisp,
                UserHookDataPtr->MilLutBuffer);
      }

   return 0;
   }

/* Allocation routine used to allocate FPGA processing buffers. */
/* ----------------------------------------------------------------------- */
void AllocateProcessingBuffers(MIL_ID MilSystem, MIL_ID MilDigitizer, MIL_ID MilDisplay,
                               MIL_INT SizeBand, MIL_INT SizeX, MIL_INT SizeY,
                               MIL_INT SizeBit, HookDataStruct* Struct)
   {
   MIL_INT DispSizeBand       = 0;
   MIL_INT64 DispAttribute    = 0;
   Struct->MilImageDisp       = M_NULL;
   Struct->WhiteBalance       = M_NULL;
   Struct->MilGainImage       = M_NULL;
   Struct->MilOffsetImage     = M_NULL;
   Struct->MilLutBuffer       = M_NULL;

   switch(Struct->ProcessingToDo)
      {
      case eBayerProcessing:
         DispSizeBand = 3;
         DispAttribute = M_IMAGE+M_GRAB+M_DISP+M_BGR32+M_PACKED+M_HOST_MEMORY+
                         M_FPGA_ACCESSIBLE;
         MbufAlloc1d(MilSystem, 3, 32+M_FLOAT, M_ARRAY, &Struct->WhiteBalance);
         Struct->BitShift = SizeBit - 8;
         /* Force display buffer to be 8 bits for bayer. */
         SizeBit = 8;
         break;
      case eOffsetGainProcessing:
         DispSizeBand = SizeBand;
         DispAttribute = M_IMAGE+M_GRAB+M_DISP+M_PROC+M_HOST_MEMORY+M_FPGA_ACCESSIBLE;
         /* The gain&Offset buffers MUST reside in the Processing Fpga SDRAM memory */
         MbufAllocColor(MilSystem,
                        SizeBand,
                        SizeX,
                        SizeY,
                        (SizeBit == 8 ? 8+M_UNSIGNED : 16+M_UNSIGNED),
                        M_IMAGE+M_PROC+M_FPGA_ACCESSIBLE,
                        &Struct->MilGainImage);
         MbufAllocColor(MilSystem,
                        SizeBand,
                        SizeX,
                        SizeY,
                        (SizeBit == 8 ? 8+M_UNSIGNED : 16+M_UNSIGNED),
                        M_IMAGE+M_PROC+M_FPGA_ACCESSIBLE,
                        &Struct->MilOffsetImage);

         /* Lets clear the offset and gain buffers to a constant for the
         purpose of this example. */
         MbufClear(Struct->MilGainImage, 16);
         MbufClear(Struct->MilOffsetImage, 2);
         break;
      case eLutMapProcessing:
         {
         MIL_INT LutSize = (MIL_INT)pow(2.0, (double)SizeBit);
         DispSizeBand = SizeBand;
         DispAttribute = M_IMAGE+M_GRAB+M_DISP+M_PROC+M_HOST_MEMORY+M_FPGA_ACCESSIBLE;
         MbufAlloc1d(MilSystem,
            LutSize,
            (SizeBit == 8 ? 8+M_UNSIGNED : 16+M_UNSIGNED),
            M_LUT+M_FPGA_ACCESSIBLE,
            &Struct->MilLutBuffer);

         /* Initialize LUT as an inverse ramp. */
         MgenLutRamp(Struct->MilLutBuffer, 0, (MIL_DOUBLE)(LutSize-1), (LutSize-1), 0.0);
         }
         break;
      }

   MbufAllocColor(MilSystem,
                  DispSizeBand,
                  SizeX,
                  SizeY,
                  (SizeBit == 8 ? 8+M_UNSIGNED : 16+M_UNSIGNED),
                  DispAttribute,
                  &Struct->MilImageDisp);

   MdispSelect(MilDisplay, Struct->MilImageDisp);
   /* Correctly remap 16 bit images to the display. */
   if(SizeBit > 8)
      {
      MdispControl(MilDisplay, M_VIEW_MODE, M_BIT_SHIFT);
      MdispControl(MilDisplay, M_VIEW_BIT_SHIFT, SizeBit-8);
      }
   }