/**********************************************************************************/
/*
 *  File name: DMILprocessingSlave.cpp
 *  Location: See Matrox Example Launcher in the MIL Control Center
 * 
 *
 *  Synopsis:  This slave function uses DMIL to do an autonomous processing
 *             task that runs directly on target Slave system. It does image 
 *             analysis and can take some time critical action immediately and 
 *             locally (Ex: I/O control) before to provide the results to the Master 
 *             for non time critical actions (Ex: Statistics and image display).
 *
 *             The master function can be found in the DMILprocessing project.
 *
 *  Copyright (C) Matrox Electronic Systems Ltd., 1992-2020.
 *  All Rights Reserved
 */

/* MIL Header. */
#include <mil.h>

/* Defines the common runtime options and shared data structures. */
#include "..\DMILprocessingMaster\DMILprocessing.h" 

/* Target model specifications. */
#define MODEL_WIDTH           128L
#define MODEL_HEIGHT          128L
#define MODEL_POS_X_INIT      320L
#define MODEL_POS_Y_INIT      240L
#define MODEL_NB_OF_OCCURENCE   1L
#define MODEL_MIN_FOUND_SCORE 50.0
#define MODEL_MIN_PASS_SCORE  70.0
#define MODEL_ANGLE_DELTA_ANGLE  10
#define MODEL_ANGLE_DELTA_POS    MODEL_ANGLE_DELTA_ANGLE
#define MODEL_ANGLE_DELTA_NEG    MODEL_ANGLE_DELTA_ANGLE
#define MODEL_ANGLE_ANGLE_STEP   2
#define MODEL_ANGLE_MIN_ACCURACY 2.00

/* Slave processing function prototype. */
#if M_MIL_USE_RT
#define MIL_SLAVE_EXPORT __declspec(dllexport)
#else
#define MIL_SLAVE_EXPORT
#endif

#ifdef __cplusplus
extern "C" {
#endif
   MIL_SLAVE_EXPORT void MFTYPE SlaveProcessingLoop(MIL_ID Func);
#ifdef __cplusplus
}
#endif

/* Slave function error code. */
#define PROCESSING_LOOP_ERROR_CODE 2

/* MdigProcess() hook function prototypes. */
MIL_INT MFTYPE PreprocessingHook(MIL_INT HookType, MIL_ID EventId, void* DataExPtr);
MIL_INT MFTYPE ProcessingHook(MIL_INT HookType, MIL_ID EventId, void* DataExPtr);

/* External IO control function prototype. */
void SetExternalIOState(MIL_INT Operation, DataExchangeStruct *DataExPtr);

/* External IO control function parameters defines. */
#define IO_INITIALIZE 1
#define IO_SET        2
#define IO_CLOSE      3


/******************************************************************************
 *  Slave function:
 *      - This slave function start a grab and processing sequence locally and
 *        autonomously. It signals to the controlling computer when results
 *        are available. This prevents the controlling computer from sending
 *        each MIL command individually, reducing the inter-computer overhead,
 *        the interrelation and the synchronization to the minimum.
 *        This also permits to communicate all the processing results at the 
 *        same time.
 *
 *        If requested by the Master and present on the Slave, IO/s will also 
 *        be set in real-time according to the processing results.
 */

void MFTYPE SlaveProcessingLoop(MIL_ID Func)
{
   /* Variable declarations */
   MIL_ID MilDataExchangeBuffer;
   DataExchangeStruct DataEx;
   MIL_INT  Iter;
   
   /* Read the parameter. */
   MfuncParamValue(Func, 1, &MilDataExchangeBuffer);

   /* Read the Data Exchange structure. */
   MbufGet(MilDataExchangeBuffer, &DataEx);

   /* Initialize I/Os if requested and available to the PC. */
   if (DataEx.IoUpdateFlag)
      SetExternalIOState(IO_INITIALIZE, &DataEx);
   
   /* -------------- */
   /* PREPROCESSING. */

   /* Allocate grab buffers. */
   for (Iter = 0; Iter < NB_TARGET_IMAGES; Iter++)
   {
      MbufAlloc2d(DataEx.MilSystem,
         MdigInquire(DataEx.MilDigitizer, M_SIZE_X, M_NULL),
         MdigInquire(DataEx.MilDigitizer, M_SIZE_Y, M_NULL),
         8L + M_UNSIGNED,
         M_IMAGE + M_GRAB + M_PROC, &DataEx.MilImage[Iter]);
   }

   /* Update the shared data structure. */
   MbufPut(DataEx.MilDataExchangeBuffer, &DataEx);

   /* Start the preprocessing sequence. */
   DataEx.NbGrabDone = 0;
   MdigProcess(DataEx.MilDigitizer, DataEx.MilImage, NB_TARGET_IMAGES,
      M_START, M_ASYNCHRONOUS, PreprocessingHook, &DataEx);

   /* Wait until the Model Position Ok Event is set by the Host. */
   MthrWait(DataEx.MilPreprocessingStopEvent, M_EVENT_WAIT, M_NULL);

   /* Stop the model positioning sequence. */
   MdigProcess(DataEx.MilDigitizer, DataEx.MilImage, NB_TARGET_IMAGES,
      M_STOP, M_SYNCHRONOUS, PreprocessingHook, &DataEx);

   /* Set the Buffer Available event to signal to the Master system 
      that the results are ready.
    */
   MthrControl(DataEx.MilDataExchangeBufferAvailableEvent, M_EVENT_SET, M_SIGNALED);

   /* Wait until the Start processing Event is set by the Host to start processing. */
   MthrWait(DataEx.MilStartProcessingEvent, M_EVENT_WAIT, M_NULL);

   /* Update the shared data structure. */
   MbufGet(DataEx.MilDataExchangeBuffer, &DataEx);

   /* Reinitialize hook structure statistics. */
   DataEx.NbProcessingDone = 0;
   DataEx.Found = 0;
   DataEx.FrameMissed = 0;
   DataEx.ProcessingTimeExceededCount = 0;
   DataEx.ProcessingTimeVariationAverage = 0;
   DataEx.ProcessingTimeExceededMax = 0;
   DataEx.ProcessingTimeVariationCurrent = 0;
   DataEx.Error = M_FALSE;

   /* --------------------------- */
   /* GRAYSCALE PATTERN MATCHING. */
   if (DataEx.ProcessingMethod == PATTERN_MATCHING)
   {
      /* Allocate normalized grayscale type model from the last grabbed image. */
      MpatAllocModel(DataEx.MilSystem, DataEx.MilImage[0],
         (MIL_INT)(MODEL_POS_X_INIT + 0.5) - (MODEL_WIDTH / 2) - 1,
         (MIL_INT)(MODEL_POS_Y_INIT + 0.5) - (MODEL_HEIGHT / 2) - 1,
         MODEL_WIDTH, MODEL_HEIGHT,
         M_NORMALIZED, &DataEx.MilModel);

      /* Allocate result. */
      MpatAllocResult(DataEx.MilSystem, 1L, &DataEx.MilResult);

      /* If no allocation error, set the model search parameters,
         preprocess the Target model and set the error state to false.
         */
      if ((DataEx.MilModel != M_NULL) && (DataEx.MilResult != M_NULL))
      {
         MpatSetNumber(DataEx.MilModel, MODEL_NB_OF_OCCURENCE);
         MpatSetAcceptance(DataEx.MilModel, MODEL_MIN_FOUND_SCORE);
         MpatSetSpeed(DataEx.MilModel, M_HIGH);
         MpatSetAccuracy(DataEx.MilModel, M_LOW);
         MpatSetAngle(DataEx.MilModel, M_SEARCH_ANGLE_DELTA_NEG, MODEL_ANGLE_DELTA_NEG);
         MpatSetAngle(DataEx.MilModel, M_SEARCH_ANGLE_DELTA_POS, MODEL_ANGLE_DELTA_POS);
         MpatSetAngle(DataEx.MilModel, M_SEARCH_ANGLE_ACCURACY, MODEL_ANGLE_MIN_ACCURACY);
         MpatPreprocModel(DataEx.MilImage[0], DataEx.MilModel, M_DEFAULT);
      }
      else
      {
         /* Report a MIL error. */
         DataEx.Error = M_TRUE;
         MfuncErrorReport(Func, M_FUNC_ERROR + PROCESSING_LOOP_ERROR_CODE,
            MIL_TEXT("Error during target processing loop model allocations."),
            M_NULL, M_NULL, M_NULL);
      }

   }
   else
   {
      if (PROCESSING_TYPE != BINARIZATION)
      {
         /* Report a MIL error. */
         DataEx.Error = M_TRUE;
         MfuncErrorReport(Func, M_FUNC_ERROR + PROCESSING_LOOP_ERROR_CODE,
            MIL_TEXT("No known processing Specified."),
            M_NULL, M_NULL, M_NULL);
      }
   }
   
   /* Start the processing sequence. */
   if (DataEx.Error == M_FALSE)
   {
      MdigProcess(DataEx.MilDigitizer, DataEx.MilImage, NB_TARGET_IMAGES,
         M_START, M_ASYNCHRONOUS, ProcessingHook, &DataEx);

      /* Here this thread is free to do any other task. The MdigProcess() function
      is running autonomously and passing the necessary results to the Host via
      the data exchange buffer.
      */

      /* Wait until the Stop Processing Event is set by the Host. */
      MthrWait(DataEx.MilStopProcessingEvent, M_EVENT_WAIT, M_NULL);

      /* Stop the processing sequence. */
      MdigProcess(DataEx.MilDigitizer, DataEx.MilImage, NB_TARGET_IMAGES,
         M_STOP, M_SYNCHRONOUS, ProcessingHook, &DataEx);
   }
   else
      {
      /* Update the shared data structure. */
      MbufPut(DataEx.MilDataExchangeBuffer, &DataEx);
      // We have an error, signal the client that the exchange buffer has changed
      MthrControl(DataEx.MilDataExchangeBufferAvailableEvent, M_EVENT_SET, M_SIGNALED);
      }

   /* Free pattern matching objects if they exist. */
   if (DataEx.ProcessingMethod == PATTERN_MATCHING)
   {
      if (DataEx.MilResult != M_NULL) MpatFree(DataEx.MilResult);
      if (DataEx.MilModel != M_NULL) MpatFree(DataEx.MilModel);
   }

   /* Close I/Os if requested. */
   if (DataEx.IoUpdateFlag)
      SetExternalIOState(IO_CLOSE, &DataEx);
}

/******************************************************************************
 *  Pre processing hook function:
 *      - This hook function is called locally every time MdigProcess does a grab
 *        to prepare the processing.
 */

MIL_INT MFTYPE PreprocessingHook(MIL_INT HookType, MIL_ID EventId, void* DataExVoidPtr)
{
   DataExchangeStruct *DataExPtr = (DataExchangeStruct *)DataExVoidPtr;
   MIL_ID GrabBufferId;

   /* Synchronize and start the timer. */
   if (DataExPtr->NbGrabDone == 0)
      MappTimer(M_DEFAULT, M_TIMER_RESET, M_NULL);

   /* Read the elapsed time. */
   MappTimer(M_DEFAULT, M_TIMER_READ, &DataExPtr->Time);

   /* Update the initial camera frame rate statistic. */
   DataExPtr->FrameRate = (MIL_DOUBLE)DataExPtr->NbGrabDone / DataExPtr->Time;

   /* Increment operation count. */
   DataExPtr->NbGrabDone++;

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

   if (DataExPtr->ProcessingMethod == PATTERN_MATCHING)
   {
      /* Draw a rectangle in the Overlay around the position of the model to define. */
      MgraRect(M_DEFAULT, GrabBufferId,
         (MIL_INT)(MODEL_POS_X_INIT + 0.5) - (MODEL_WIDTH / 2) - 2,
         (MIL_INT)(MODEL_POS_Y_INIT + 0.5) - (MODEL_HEIGHT / 2) - 2,
         (MIL_INT)(MODEL_POS_X_INIT + 0.5) + (MODEL_WIDTH / 2) + 1,
         (MIL_INT)(MODEL_POS_Y_INIT + 0.5) + (MODEL_HEIGHT / 2) + 1);
   }
   else
   { /* Print a message. */
      MgraText(M_DEFAULT, GrabBufferId,
         MODEL_POS_X_INIT, MODEL_POS_Y_INIT, MIL_TEXT("<<<< LIVE IMAGE >>>>"));
   }

   /* Copy the image to the display. */
   MbufCopy(GrabBufferId, DataExPtr->MilImageDisp);

   /* Write the updated statistic. */
   MbufPut(DataExPtr->MilDataExchangeBuffer, DataExPtr);

   /* Check for error on the Slave to report it to the Master. */
   if (MappGetError(M_DEFAULT, M_CURRENT, M_NULL))
      DataExPtr->Error = M_TRUE;
   else
      DataExPtr->Error = M_FALSE;
   
#if M_MIL_USE_CE
   /* Give execution time to user interface when the digitizer processing queue is full.
      If necessary, the Sleep value can be increased to give more execution time to user
      interface.
      */
   if(MdigInquire(DataExPtr->MilDigitizer, M_PROCESS_PENDING_GRAB_NUM, M_NULL) <= 1)
   {
      if ((DataExPtr->NbProcessingDone%10) == 0)
         Sleep(2);
   }
#endif

   return (M_NULL);
}


/******************************************************************************
 *  Processing hook function:
 *      - This hook function is called locally every time MdigProcess does a grab.
 *        This avoid to the Host computer to send each processing command individually,
 *        reducing the inter-computer overhead. It also signals to the host when all the
 *        results are available.
 */

MIL_INT MFTYPE ProcessingHook(MIL_INT HookType, MIL_ID EventId, void* DataExVoidPtr)
{
   DataExchangeStruct *DataExPtr = (DataExchangeStruct *)DataExVoidPtr;
   MIL_ID GrabBufferId;
   MIL_INT NbFound, PreviousFrameMissed, Threshold;
   MIL_DOUBLE PreviousTime = 0;

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

   /* Synchronize and start the timer. */
   if (DataExPtr->NbProcessingDone == 0)
   {
      MappTimer(M_DEFAULT, M_TIMER_RESET, M_NULL);
      MappTimer(M_DEFAULT, M_TIMER_READ, &DataExPtr->Time);
   }
   /* Read the elapsed time and verify that the Camera frame rate is respected 
      within the specified margin.
    */
   else
   {
      /* Update the statistics. */
      PreviousTime = DataExPtr->Time;
      MappTimer(M_DEFAULT, M_TIMER_READ, &DataExPtr->Time);
      DataExPtr->ProcessingTimeVariationCurrent = (DataExPtr->Time - PreviousTime) - 
                                                  (1 / DataExPtr->FrameRate);
      if (DataExPtr->ProcessingTimeVariationCurrent < 0)
         DataExPtr->ProcessingTimeVariationCurrent = 0;
      if (DataExPtr->ProcessingTimeVariationCurrent > DataExPtr->ProcessingTimeExceededMax)
         DataExPtr->ProcessingTimeExceededMax = DataExPtr->ProcessingTimeVariationCurrent;
      DataExPtr->ProcessingTimeVariationAverage =
         ((DataExPtr->ProcessingTimeVariationAverage * (DataExPtr->NbProcessingDone)) +
         DataExPtr->ProcessingTimeVariationCurrent) / (DataExPtr->NbProcessingDone + 1);

      /* If Late call back: Set status and write a Warning. */
      if (DataExPtr->ProcessingTimeVariationCurrent > DataExPtr->MaxJitterAllowed)
      {
         DataExPtr->Late = M_TRUE;
         DataExPtr->ProcessingTimeExceededCount++;
         MgraText(M_DEFAULT, GrabBufferId,
            MODEL_POS_X_INIT - 120, MODEL_POS_Y_INIT + 100, 
            MIL_TEXT("<<<< WARNING: PROCESSING WAS LATE >>>>"));
      }
      else
      {
         DataExPtr->Late = M_FALSE;
      }
   }

   /* If frame missed: Set status and write a Warning. */
   PreviousFrameMissed = DataExPtr->FrameMissed;
   MdigInquire(DataExPtr->MilDigitizer, M_PROCESS_FRAME_MISSED, &DataExPtr->FrameMissed);
   if (DataExPtr->FrameMissed > PreviousFrameMissed)
   {
      DataExPtr->Late = M_TRUE;
      MgraText(M_DEFAULT, GrabBufferId,
         MODEL_POS_X_INIT - 120, MODEL_POS_Y_INIT + 150, 
         MIL_TEXT("<<<< WARNING: FRAME WAS MISSED >>>>"));
   }

   /* If PATTERN MATCHING is selected. */
   if (DataExPtr->ProcessingMethod == PATTERN_MATCHING)
   {
      /* Find the model. */
      MpatFindModel(GrabBufferId, DataExPtr->MilModel, DataExPtr->MilResult);

      /* Increment find operation count. */
      DataExPtr->NbProcessingDone++;

      /* Get the results. */
      MpatGetNumber(DataExPtr->MilResult, &NbFound);
      DataExPtr->Found = NbFound;
      MpatGetResult(DataExPtr->MilResult, M_POSITION_X, &DataExPtr->PosX);
      MpatGetResult(DataExPtr->MilResult, M_POSITION_Y, &DataExPtr->PosY);
      MpatGetResult(DataExPtr->MilResult, M_SCORE, &DataExPtr->Score);
   }
   else /* GENERAL PROCESSING. */
   {
      /* Process the image (in place) and find if enough contrast was present. */
      Threshold = MimBinarize(GrabBufferId, M_NULL, M_BIMODAL + M_GREATER, M_NULL, M_NULL);
      MimBinarize(GrabBufferId, GrabBufferId, M_BIMODAL + M_GREATER, M_NULL, M_NULL);

      /* Draw a message. */
      MgraText(M_DEFAULT, GrabBufferId,
         MODEL_POS_X_INIT, MODEL_POS_Y_INIT, MIL_TEXT("<<<< BINARIZED IMAGE >>>>"));

      /* Increment operation count. */
      DataExPtr->NbProcessingDone++;

      /* Set the results by simulating a score (100% if threshold point is 128). */
      if (Threshold <= 128)
         DataExPtr->Score = (MIL_DOUBLE)(Threshold * 100) / 128;
      else
         DataExPtr->Score = (MIL_DOUBLE)((255-Threshold) * 100) / 128;
      if (DataExPtr->Score > 50)
         DataExPtr->Found = 1;
      else
         DataExPtr->Found = 0;
   }

   /* Update the Status. */
   if (DataExPtr->Found) // Model was found.
   {
      if (DataExPtr->Score > MODEL_MIN_PASS_SCORE) 
         DataExPtr->Status = PASS;
      else 
         DataExPtr->Status = WARNING;
   }
   else 
      DataExPtr->Status = FAIL;

   /* Update IO if required. */
   if (DataExPtr->IoUpdateFlag)
      SetExternalIOState(IO_SET, DataExPtr);
   
   /* Check if the previous results were processed by the Master otherwise
      skip this update in order to not lose time waiting and stay Real-time.
      */
   if (MthrInquire(DataExPtr->MilDataExchangeBufferProcessedEvent, M_EVENT_STATE, M_NULL) 
                                                                           == M_SIGNALED)
   {
      /* Reset the Buffer processed event to signal to the Target system have seen it. */
      MthrControl(DataExPtr->MilDataExchangeBufferProcessedEvent, M_EVENT_SET, M_NOT_SIGNALED);

      /* Write the new results. */
      MbufPut(DataExPtr->MilDataExchangeBuffer, DataExPtr);

      /* If required, update the display with the processed image. */
      if (DataExPtr->DisplayUpdateFlag)
      {
         MbufCopy(GrabBufferId, DataExPtr->MilImageResult);
         if (DataExPtr->ProcessingMethod == PATTERN_MATCHING)
         {
            if (DataExPtr->Found)
               MpatDraw(M_DEFAULT, DataExPtr->MilResult, DataExPtr->MilImageResult,
               M_DRAW_BOX + M_DRAW_POSITION, M_DEFAULT, M_DEFAULT);
            else
               MgraText(M_DEFAULT, DataExPtr->MilImageResult,
               MODEL_POS_X_INIT, MODEL_POS_Y_INIT, MIL_TEXT("  MODEL NOT FOUND ?  "));
         }
         else
         {
            if (!DataExPtr->Found)
               MgraText(M_DEFAULT, DataExPtr->MilImageResult,
               MODEL_POS_X_INIT, MODEL_POS_Y_INIT + 50, MIL_TEXT("  BAD CONTRAST ?  "));
         }
      }

      /* Set the Buffer Available event to signal to the Master system that the 
         results are ready.
       */
      MthrControl(DataExPtr->MilDataExchangeBufferAvailableEvent, M_EVENT_SET, M_SIGNALED);
   }

   /* Check for error on the Slave to report it to the Master. */
   if (MappGetError(M_DEFAULT, M_GLOBAL, M_NULL))
      DataExPtr->Error = M_TRUE;
   else
      DataExPtr->Error = M_FALSE;

#if M_MIL_USE_CE
   /* Give execution time to user interface when the digitizer processing queue is full.
      If necessary, the Sleep value can be increased to give more execution time to user
      interface.
      */
   if(MdigInquire(DataExPtr->MilDigitizer, M_PROCESS_PENDING_GRAB_NUM, M_NULL) <= 1)
   {
      if ((DataExPtr->NbProcessingDone%10) == 0)
         Sleep(2);
   }
#endif

   return (M_NULL);
}


/******************************************************************************
 *  Function that would set some I/Os accessible to the Slave or do some 
 *  action according to the processing results.
 * 
 *  This could be I/Os directly on the PC or controled by
 *  an industrial protocol such as Profinet, Modbus, EtherCat or 
 *  even an industrial Robot.
 *
 *  Example of I/O assignment at run time for that example.
 *  Green light  = PASS 
 *  Yellow light = WARNING
 *  Red light    = FAIL (eject)
 *  Blue light   = LATE PROCESSING (too much latency).
 *
 *   The code below assumes a Matrox 4Sight industrial PC with embedded 
 *   auxiliary IO controlable via the MIL API.
 */
void SetExternalIOState(MIL_INT Operation, DataExchangeStruct *DataExPtr)
{

/* Control external IOs if enabled. */
#if (IO_UPDATE == M_YES) 
   /* IO control code. */
   /* ---------------- */
   if (DataExPtr->IoUpdateFlag)
   {
      /* IO Initialization: */
      if (Operation == IO_INITIALIZE)
      {
         /* Connect/Initialization code (Ex: All Lights On). */;
      }

      /* Set IOs state here: */
      if (Operation == IO_SET)
      {
         if (DataExPtr->Status == M_PASS)
         {
            /* GOOD (Ex: Green Light On, others Off). */;
         }
         else
            if (DataExPtr->Status == M_WARNING)
            {
               /* WARNING (Ex: Yellow Light On, others Off). */;
            }
            else
            {
               if (DataExPtr->Status == M_FAIL)
               {
                  /* BAD (Ex: Red Light On, others Off). */;
               }
            }

         /* Signal delayed processing. */
         if (DataExPtr->Late == M_TRUE)
         {
            /* LATE Processing or missed frame (Ex: Blue Light On. */;
         }
      }

      /* IO closing: */
      if (Operation == IO_CLOSE)
      {
         /* Diconnect code (Ex: All Lights Off.  */;
      }
   }
#endif // IO_UPDATE

}