/*************************************************************************/
/*
* File name: FdkOffsetGainLutMapPrimitive.cpp
* Location: See Matrox Example Launcher in the MIL Control Center
 * 
*
* Synopsis:  This example shows how to create an FPGA accelerated processing  
 *           function cascasding 2 processing units using the Mfpga API. 
 *           It also includes MIL Master and Slave functions that will be 
 *           used to call the processing primitive.
 *
 *           These Master and Slave functions allow the primitive to 
 *           become a true MIL function that does minimal parameter checking, 
 *           can log MIL errors and will make the custom FPGA function 
 *           visible to the MIL trace mechanism.
 *
 * Copyright (C) Matrox Electronic Systems Ltd., 1992-2020.
 * All Rights Reserved
 */

/* Headers. */
#include <mil.h>
#include <milfpga.h>
#include <fpga_gainoffset.h>
#include <fpga_lutmap.h>

/* Functions definition */
#define FUNCTION_NB_PARAM_LOAD_LUT              1
#define FUNCTION_OPCODE_FPGA_LOAD_LUT           3
#define FUNCTION_NB_PARAM_OFFSET_GAIN_LUT       6
#define FUNCTION_OPCODE_FPGA_OFFSET_GAIN_LUT    4

/* Error codes */
#define FUNCTION_PARAMETER_ERROR_CODE           1
#define FUNCTION_WRONG_FPGA_ERROR_CODE          2

/* Master, Slaves and Primitives MIL functions declarations. */
void FPGALoadLut(MIL_ID LutId);
void MFTYPE FPGALoadLutSlave(MIL_ID Func);
void FPGALoadLutPrimitive(MIL_ID Func, MIL_ID MilLut);
void MFTYPE FPGAOffsetGainLutMapSlave(MIL_ID Func);
void FPGAOffsetGainLutMapPrimitive(MIL_ID  Func, MIL_ID  SrcImageId,
     MIL_ID OffsetImageId, MIL_ID GainImageId, MIL_INT Src4, MIL_ID LutId, MIL_ID  DstImageId);
void FPGAInitializeShadowRegisters(MIL_ID Func, FPGA_GAINOFFSET_USER_ST* pOGShadow,
     FPGA_LUTMAP_USER_ST*pLMShadow, MIL_BUFFER_INFO Src1, MIL_BUFFER_INFO Src2,
     MIL_BUFFER_INFO Src3, MIL_INT Src4, MIL_BUFFER_INFO Lut, MIL_BUFFER_INFO Dst);

/* MIL Load LUT primitive function definition. */
/* ------------------------------------------- */

void FPGALoadLutPrimitive(MIL_ID Func, MIL_ID LutId)
   {
   MIL_BUFFER_INFO Lut;
   MIL_FPGA_CONTEXT LutMapContext;
   FPGA_LUTMAP_USER_ST LMShadow;
   memset(&LMShadow, 0, sizeof(LMShadow));

   /* Inquire the buffer info object for the LUT buffer. */
   MfuncInquire(LutId, M_BUFFER_INFO, &Lut);

   /* Allocate an asynchronous command context on the LutMap processing unit. */
   if(MfpgaCommandAlloc(MfuncBufOwnerSystemId(Lut), M_DEV0, FPGA_LUTMAP_FID,
         M_DEFAULT, M_DEV0, M_ASYNCHRONOUS, M_DEFAULT, &LutMapContext))
      {
      /* Program the actual number of bits in the lut. */
      switch(MfuncBufSizeX(Lut))
         {
         case 256:
            LMShadow.ctrl.f.lutbits = 0;  break;
         case 512:
            LMShadow.ctrl.f.lutbits = 1;  break;
         case 1024:
            LMShadow.ctrl.f.lutbits = 2;  break;
         case 2048:
            LMShadow.ctrl.f.lutbits = 3;  break;
         case 4096:
            LMShadow.ctrl.f.lutbits = 4;  break;
         default:
            MfuncErrorReport( 
                Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
                MIL_TEXT("Invalid LUT depth."),
                M_NULL, M_NULL, M_NULL
                );
         }

      /* This is a load lut operation. */
      LMShadow.ctrl.f.mode = 0;

      /* Input0 of the Lut processing unit is connected to the Lut buffer. */
      MfpgaSetSource(LutMapContext, Lut, M_INPUT0, M_DEFAULT);

      /* Pass the initialized shadow register structure, it will be programmed
         when the processing operation is dispatched (before processing
         starts).
      */
      MfpgaSetRegister(LutMapContext, M_USER, 0, sizeof(LMShadow),
         (void*)(&LMShadow), M_WHEN_DISPATCHED);

      /* Because this command context was allocated in asynchronous mode,
         MfpgaCommandQueue will return immediately.
      */
      MfpgaCommandQueue(LutMapContext, M_DEFAULT, M_DEFAULT);

      /* Free the allocated context. */
      MfpgaCommandFree(LutMapContext, M_DEFAULT);
      }
   else
      {
      /* Report a MIL error. */
      MfuncErrorReport( 
          Func, M_FUNC_ERROR+FUNCTION_WRONG_FPGA_ERROR_CODE,
          MIL_TEXT("No LutMap acceleration supported by this FPGA configuration."),
          MIL_TEXT("Please select the proper FPGA configuration file."),
          MIL_TEXT("See the processing FPGA section of the milconfig utility."),
          M_NULL
          );
      }
   }

/* MIL Offset Gain and LUT map primitive function definition. */
/* ---------------------------------------------------------- */

/***************************************************************
**                         _______       _______
** SrcImage0         ---->| gain  |     | lut   |
** SrcImage1(Offset) ---->| offset|---->| map   |---->DstImage
** SrcImage2(Gain)   ---->|_______|     |_______|
**                            ^
**                            |
**               Src4 (normalization factor register)
**
** DstImage = Lut[ (((SrcImage0-SrcImage1)*SrcImage3)/Src4) ]
**
*****************************************************************/
void FPGAOffsetGainLutMapPrimitive(MIL_ID Func, MIL_ID SrcImageId,
                                   MIL_ID OffsetImageId, MIL_ID GainImageId, 
                                   MIL_INT Src4, MIL_ID  LutId, MIL_ID  DstImageId)
   {
   MIL_BUFFER_INFO Src, Offset, Gain, Lut, Dst;
   MIL_FPGA_CONTEXT OffsetGainContext, LutMapContext;
   FPGA_GAINOFFSET_USER_ST GOShadow;
   FPGA_LUTMAP_USER_ST LMShadow;
   memset(&GOShadow, 0, sizeof(GOShadow));
   memset(&LMShadow, 0, sizeof(LMShadow));

   /* Inquire the buffer info object of all the buffers. */
   MfuncInquire(SrcImageId, M_BUFFER_INFO, &Src);
   MfuncInquire(OffsetImageId, M_BUFFER_INFO, &Offset);
   MfuncInquire(GainImageId, M_BUFFER_INFO, &Gain);
   MfuncInquire(LutId, M_BUFFER_INFO, &Lut);
   MfuncInquire(DstImageId, M_BUFFER_INFO, &Dst);

   /* Allocate an asynchronous command context on the OffsetGain processing unit. */
   if(MfpgaCommandAlloc(MfuncBufOwnerSystemId(Src), M_DEV0, FPGA_GAINOFFSET_FID,
                        M_DEFAULT, M_DEV0, M_ASYNCHRONOUS, M_DEFAULT, &OffsetGainContext))
      {
      /* Allocate an asynchronous command context on the LutMap processing unit. */
      if(MfpgaCommandAlloc(MfuncBufOwnerSystemId(Src), M_DEV0, FPGA_LUTMAP_FID,
                           M_DEFAULT, M_DEV0, M_ASYNCHRONOUS, M_DEFAULT, &LutMapContext))
         {
         /* Initialize the shadow register structure. */
         FPGAInitializeShadowRegisters(Func, &GOShadow, &LMShadow, Src, 
                                       Offset, Gain, Src4, Lut, Dst);

         /* Input0 of OffsetGain processing unit is connected to the Src buffer. */
         MfpgaSetSource(OffsetGainContext, Src, M_INPUT0, M_DEFAULT);

         /* Input1 of OffsetGain processing unit is connected to the Offset buffer. */
         MfpgaSetSource(OffsetGainContext, Offset, M_INPUT1, M_DEFAULT);

         /* Input2 of OffsetGain processing unit is connected to the Gain buffer. */
         MfpgaSetSource(OffsetGainContext, Gain, M_INPUT2, M_DEFAULT);

         /* Pass the initialized shadow register structure, it will be programmed 
            when the processing operation is dispatched (before processing starts). 
          */
         MfpgaSetRegister(OffsetGainContext, M_USER, 0, sizeof(GOShadow),
                          (void*)(&GOShadow), M_WHEN_DISPATCHED);

         /* Output0 of the LutMap processing unit is connected to the Dest buffer. */
         MfpgaSetDestination(LutMapContext, Dst, M_OUTPUT0, M_DEFAULT);

         /* Pass the initialized shadow register structure, it will be programmed
            when the processing operation is dispatched (before processing starts). */
         MfpgaSetRegister(LutMapContext, M_USER, 0, sizeof(LMShadow),
                          (void*)(&LMShadow), M_WHEN_DISPATCHED);

         /* Output0 of the OffsetGain is connected to Input0 of the LutMap. */
         MfpgaSetLink(OffsetGainContext, M_OUTPUT0, LutMapContext, M_INPUT0, M_DEFAULT);

         /* Queue the processing operation, we use the M_WAIT flag because
            other MfpgaCommandQueue follows. This lets the software know
            that it must wait for other MfpgaCommandQueue operations. */
         MfpgaCommandQueue(LutMapContext, M_DEFAULT, M_WAIT);

         /* Queue the processing operation, we use the M_DEFAULT flag. 
            This lets the software know that it can send the processing
            operation to the FPGA. */
         MfpgaCommandQueue(OffsetGainContext, M_DEFAULT, M_DEFAULT);

         /* Free the allocated context. */
         MfpgaCommandFree(OffsetGainContext, M_DEFAULT);
         MfpgaCommandFree(LutMapContext, M_DEFAULT);
         }
      else
         {
         /* Report a MIL error. */
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_WRONG_FPGA_ERROR_CODE,
             MIL_TEXT("No LutMap acceleration supported by this FPGA configuration."),
             MIL_TEXT("Please select the proper FPGA configuration file."),
             MIL_TEXT("See the processing FPGA section of the milconfig utility."),
             M_NULL
             );
         }
      }
   else
      {
      /* Report a MIL error. */
      MfuncErrorReport( 
          Func, M_FUNC_ERROR+FUNCTION_WRONG_FPGA_ERROR_CODE,
          MIL_TEXT("No OffsetGain acceleration supported by this FPGA configuration."),
          MIL_TEXT("Please select the proper FPGA configuration file."),
          MIL_TEXT("See the processing FPGA section of the milconfig utility."),
          M_NULL
          );
      }
   }

/* MIL PU Register initialization function definition. */
/* ---------------------------------- */

void FPGAInitializeShadowRegisters( MIL_ID Func,
                                    FPGA_GAINOFFSET_USER_ST* pOGShadow,
                                    FPGA_LUTMAP_USER_ST* pLMShadow,
                                    MIL_BUFFER_INFO Src1,
                                    MIL_BUFFER_INFO Src2,
                                    MIL_BUFFER_INFO Src3,
                                    MIL_INT         Src4,
                                    MIL_BUFFER_INFO Lut,
                                    MIL_BUFFER_INFO Dst)
   {
   MIL_UINT lMaxValue, Src1Type, Src2Type, Src3Type, DstType, i, j;
   lMaxValue = (MIL_UINT)MfuncBufMaxValue(Src1);
   Src1Type = MfuncBufType(Src1);
   Src2Type = MfuncBufType(Src2);
   Src3Type = MfuncBufType(Src3);
   DstType = MfuncBufType(Dst);

   /*****************************
   ** Set OffsetGain registers
   *****************************/

   /*Set in-out types. */
   switch(Src1Type)
      {
      case 8+M_UNSIGNED:
         pOGShadow->ctrl.f.srctype = 0x1; break;
      case 16+M_UNSIGNED:
         pOGShadow->ctrl.f.srctype = 0x3; break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid source buffer type."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   switch(Src2Type)
      {
      case 8+M_UNSIGNED:
         pOGShadow->ctrl.f.offtype = 0x1; break;
      case 16+M_UNSIGNED:
         pOGShadow->ctrl.f.offtype = 0x3; break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid offset buffer type."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   switch(Src3Type)
      {
      case 8+M_UNSIGNED:
         pOGShadow->ctrl.f.gaintype = 0x1;   break;
      case 16+M_UNSIGNED:
         pOGShadow->ctrl.f.gaintype = 0x3;   break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid gain buffer type."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   switch(DstType)
      {
      case 8+M_UNSIGNED:
         pOGShadow->ctrl.f.dsttype = 0x1;   break;
      case 16+M_UNSIGNED:
         pOGShadow->ctrl.f.dsttype = 0x3;   break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid destination buffer type."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   /* Set the value we want to clip to. */
   pOGShadow->clipval.f.clipval  = lMaxValue;

   /* Compute the division factor. */
   j=1;
   for(i=0; i<32; i++)
      {
      if(Src4 & j)
         {
         pOGShadow->ctrl.f.shift = i;
         break;
         }
      j <<= 1;
      }

   /*****************************
   ** Set Lutmap registers
   *****************************/

   switch(MfuncBufSizeBit(Dst))
      {
      case 8:
         pLMShadow->ctrl.f.dsttype = 0;   break;
      case 16:
         pLMShadow->ctrl.f.dsttype = 1;   break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid destination buffer type."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   switch(MfuncBufSizeBit(Src1))
      {
      case 8:
         pLMShadow->ctrl.f.srctype = 0;   break;
      case 16:
         pLMShadow->ctrl.f.srctype = 1;   break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid source buffer type."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   switch(MfuncBufSizeX(Lut))
      {
      case 256:
         pLMShadow->ctrl.f.lutbits = 0;   break;
      case 512:
         pLMShadow->ctrl.f.lutbits = 1;   break;
      case 1024:
         pLMShadow->ctrl.f.lutbits = 2;   break;
      case 2048:
         pLMShadow->ctrl.f.lutbits = 3;   break;
      case 4096:
         pLMShadow->ctrl.f.lutbits = 4;   break;
      default:
         MfuncErrorReport( 
             Func, M_FUNC_ERROR+FUNCTION_PARAMETER_ERROR_CODE,
             MIL_TEXT("Invalid lut depth."),
             M_NULL, M_NULL, M_NULL
             );
         break;
      }

   /* This is a Lutmap operation. */
   pLMShadow->ctrl.f.mode = 1;
   }

/* MIL Master function definition. */
/* ------------------------------- */
void FPGALoadLut(MIL_ID LutId)
   {
   MIL_ID   Func;

   /* Allocate a MIL function context that will be used to call a target 
      Slave function locally on the Host to do the processing.
   */
   MfuncAlloc(MIL_TEXT("FPGALoadLut"), 
              FUNCTION_NB_PARAM_LOAD_LUT,
              FPGALoadLutSlave, M_NULL, M_NULL, 
              M_USER_MODULE_1+FUNCTION_OPCODE_FPGA_LOAD_LUT, 
              M_DEFAULT, 
              &Func
             );

   /* Register the parameters. */
   MfuncParamMilId(Func, 1, LutId, M_LUT, M_IN);

   /* Call the target Slave function. */
   MfuncCall(Func);

   /* Free the MIL function context. */
   MfuncFree(Func);
   }

/* MIL Slave function definition. */
/* ------------------------------ */
void MFTYPE FPGALoadLutSlave(MIL_ID Func)
   {
   MIL_ID LutId;

   /* Read the parameters. */
   MfuncParamValue(Func, 1, &LutId);
   
   /* Call the primitive controling the processing FPGA using the Mfpga API. */
   FPGALoadLutPrimitive(Func, LutId);
   }

/* MIL Master function definition. */
/* ------------------------------- */
void FPGAOffsetGainLutMap(MIL_ID  SrcImageId, MIL_ID  OffsetImageId,
    MIL_ID GainImageId, MIL_INT Src4, MIL_ID  LutId, MIL_ID  DstImageId)
   {
   MIL_ID   Func;

   /* Allocate a MIL function context that will be used to call a target 
      Slave function locally on the Host to do the processing.
   */
   MfuncAlloc(MIL_TEXT("FPGAOffsetGainLutMap"), 
              FUNCTION_NB_PARAM_OFFSET_GAIN_LUT,
              FPGAOffsetGainLutMapSlave, M_NULL, M_NULL, 
              M_USER_MODULE_1+FUNCTION_OPCODE_FPGA_OFFSET_GAIN_LUT, 
              M_DEFAULT, 
              &Func
             );

   /* Register the parameters. */
   MfuncParamMilId (Func, 1, SrcImageId,    M_IMAGE, M_IN);
   MfuncParamMilId (Func, 2, OffsetImageId, M_IMAGE, M_IN);
   MfuncParamMilId (Func, 3, GainImageId,   M_IMAGE, M_IN);
   MfuncParamMilInt(Func, 4, Src4);
   MfuncParamMilId (Func, 5, LutId,         M_LUT,   M_IN);
   MfuncParamMilId (Func, 6, DstImageId,    M_IMAGE, M_OUT);

   /* Call the target Slave function. */
   MfuncCall(Func);

   /* Free the MIL function context. */
   MfuncFree(Func);
   }

/* MIL Slave function definition. */
/* ------------------------------ */
void MFTYPE FPGAOffsetGainLutMapSlave(MIL_ID Func)
   {
   MIL_ID SrcImageId, OffsetImageId, GainImageId, LutId, DstImageId;
   MIL_INT Src4;

   /* Read the parameters. */
   MfuncParamValue(Func, 1, &SrcImageId);
   MfuncParamValue(Func, 2, &OffsetImageId); 
   MfuncParamValue(Func, 3, &GainImageId);
   MfuncParamValue(Func, 4, &Src4);
   MfuncParamValue(Func, 5, &LutId);
   MfuncParamValue(Func, 6, &DstImageId);
    
   /* Call the primitive controling the processing FPGA using the Mfpga API. */
   FPGAOffsetGainLutMapPrimitive(Func, SrcImageId, OffsetImageId, GainImageId, Src4, LutId, DstImageId);
   }