/*****************************************************************************/
/* 
 * File name: MdigAutoFocus.c
 *
 * Synopsis:  This program performs an autofocus operation using the 
 *            MdigFocus() function. Since the way to move a motorized
 *            camera lens is device-specific, we will not include real
 *            lens movement control and image grab but will simulate 
 *            the lens focus with a smooth operation. 
 */
#include <mil.h>
#include <conio.h>
#include <stdlib.h>

/* Source MIL image file specification. */ 
#define IMAGE_FILE                     M_IMAGE_PATH MIL_TEXT("BaboonMono.mim")

/* Lens mechanical characteristics. */
#define FOCUS_MAX_NB_POSITIONS         100
#define FOCUS_MIN_POSITION             0
#define FOCUS_MAX_POSITION             (FOCUS_MAX_NB_POSITIONS - 1)
#define FOCUS_START_POSITION           10

/* Autofocus search properties. */
#define FOCUS_MAX_POSITION_VARIATION   M_DEFAULT
#define FOCUS_MODE                     M_SMART_SCAN
#define FOCUS_SENSITIVITY              1

/* User Data structure definition. */
typedef struct 
{  MIL_ID      SourceImage;
   MIL_ID      FocusImage;
   MIL_ID      Display;
   long        Iteration;
}  DigHookUserData;

/* Autofocus callback function responsible for moving the lens. */
long MFTYPE MoveLensHookFunction(long HookType, long position, void MPTYPE *UserDataHookPtr);

/* Simulate a grab from a camera at different lens positions. */
void SimulateGrabFromCamera(MIL_ID SourceImage, MIL_ID FocusImage, long Iteration, MIL_ID AnnotationDisplay);

/* Draw position of the lens. */
void DrawCursor(MIL_ID AnnotationImage, long Position);

/*****************************************************************************/
/*  Main application function.                                               */
void main(void)
{
   MIL_ID MilApplication,                   /* Application identifier.       */
          MilSystem,                        /* System identifier.            */
          MilDisplay,                       /* Display identifier.           */
          MilSource,                        /* Source image                  */
          MilCameraFocus;                   /* Focus simulated image         */
   long   FocusPos;                         /* Best focus position           */
   DigHookUserData UserData;                /* User data passed to the hook  */

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

   /* Load the source image. */
   MbufRestore(IMAGE_FILE, MilSystem, &MilSource);
   MbufRestore(IMAGE_FILE, MilSystem, &MilCameraFocus);
   MbufClear(MilCameraFocus, 0);

   /* Select image on the display. */ 
   MdispSelect(MilDisplay, MilCameraFocus);

   /* Simulate the first image grab. */
   SimulateGrabFromCamera(MilSource, MilCameraFocus, FOCUS_START_POSITION, MilDisplay);

   /* Initialize user data needed within the hook function. */
   UserData.SourceImage = (long)MilSource;
   UserData.FocusImage  = (long)MilCameraFocus;
   UserData.Iteration   = (long)0L;
   UserData.Display     = (long)MilDisplay;

   /* Pause to show the original image. */
   printf("\nAUTOFOCUS:\n");
   printf("----------\n\n");
   printf("Automatic focusing operation will be done on this image.\n");
   printf("Press <Enter> to continue.\n\n");  
   getch();
   printf("Autofocusing...\n\n");  
  
   /* Perform Autofocus. 
      Since lens movement is hardware specific, no digitizer is used here.
      We simulate the lens movement with by smoothing the image data in 
      the hook function instead.
   */ 
   MdigFocus(M_NULL,
             MilCameraFocus,
             M_DEFAULT,
             MoveLensHookFunction,
             &UserData,
             FOCUS_MIN_POSITION,
             FOCUS_START_POSITION,
             FOCUS_MAX_POSITION,
             FOCUS_MAX_POSITION_VARIATION,
             FOCUS_MODE + FOCUS_SENSITIVITY,
             &FocusPos); 

   /* Print the best focus position and number of iterations. */
   printf("The best focus position is %d.\n", FocusPos);
   printf("The best focus position found in %d iterations.\n\n", UserData.Iteration);
   printf("Press <Enter> to end.\n");  
   getch();

  /* Free all allocations. */
   MbufFree(MilSource);
   MbufFree(MilCameraFocus);
   MappFreeDefault(MilApplication, MilSystem, MilDisplay, M_NULL, M_NULL);
}

/********************************************************************************/
/* Autofocus hook function responsible to move the lens.                        */

long MFTYPE MoveLensHookFunction(long HookType, long Position,  void MPTYPE *UserDataHookPtr)
{
 DigHookUserData *UserData = (DigHookUserData *)UserDataHookPtr;

   /* Here, the lens position must be changed according to the Position parameter.
      In that case, we simulate the lens position change followed by a grab.
   */
   if(HookType == M_CHANGE || HookType == M_ON_FOCUS)
      {
      SimulateGrabFromCamera( UserData->SourceImage, 
                              UserData->FocusImage, 
                              Position,
                              UserData->Display 
                              );
      UserData->Iteration++;
      }

   return 0;
}

/***********************************************************************************/
/* Utility function to simulate a grab from a camera at different lens position    */
/* by smoothing the original image. It should be replaced with a true camera grab. */

/* Lens simulation characteristics. */
#define FOCUS_BEST_POSITION            (FOCUS_MAX_NB_POSITIONS/2)

void SimulateGrabFromCamera(MIL_ID SourceImage, MIL_ID FocusImage, 
                            long Iteration, long AnnotationDisplay)
{
   long NbSmoothNeeded;      /* Number of smooths needed          */
   long BufType;             /* Buffer type                       */
   long BufSizeX;            /* Buffer size X                     */
   long BufSizeY;            /* Buffer size Y                     */
   long Smooth;              /* Smooth index                      */
   MIL_ID TempBuffer;        /* Temporary buffer                  */
   MIL_ID SourceOwnerSystem; /* Owner system of the source buffer */

   /* Compute number of smooths needed to simulate focus. */
   NbSmoothNeeded = abs(Iteration - FOCUS_BEST_POSITION);

   /* Buffer inquires. */
   BufType        = MbufInquire(FocusImage, M_TYPE,   M_NULL);
   BufSizeX       = MbufInquire(FocusImage, M_SIZE_X, M_NULL);
   BufSizeY       = MbufInquire(FocusImage, M_SIZE_Y, M_NULL);

   if(NbSmoothNeeded == 0)
      {
      /* Directly copy image source to destination. */
      MbufCopy(SourceImage, FocusImage);
      }
   else if(NbSmoothNeeded == 1)
      {
      /* Directly convolve image from source to destination. */
      MimConvolve(SourceImage, FocusImage, M_SMOOTH);
      }
   else 
      {
      SourceOwnerSystem = MbufInquire(SourceImage, M_OWNER_SYSTEM, M_NULL);

      /* Allocate temporary buffer. */   
      MbufAlloc2d(SourceOwnerSystem, BufSizeX, BufSizeY, 
                  BufType, M_IMAGE+M_PROC, &TempBuffer);
   
      /* Perform first smooth. */
      MimConvolve(SourceImage, TempBuffer, M_SMOOTH);
         
      /* Perform smooths. */
      for(Smooth=1;Smooth<NbSmoothNeeded-1;Smooth++)
         {
         MimConvolve(TempBuffer, TempBuffer, M_SMOOTH);
         }

      /* Perform last smooth. */
      MimConvolve(TempBuffer, FocusImage, M_SMOOTH);

      /* Free temporary buffer. */
      MbufFree(TempBuffer);
      }

   /* Draw position cursor. */
   DrawCursor(AnnotationDisplay, Iteration);
}

/****************************************************************/
/* Draw position of the focus lens.                             */

/* Cursor specifications. */
#define CURSOR_POSITION                ((BufSizeY*7)/8)
#define CURSOR_SIZE                    14
#define CURSOR_COLOR                   M_COLOR_GREEN

void DrawCursor(MIL_ID AnnotationDisplay, long Position)
{
   MIL_ID AnnotationImage;
   long CursorColor, BufSizeX, BufSizeY, n;

   /* Prepare for overlay annotations. */
   MdispControl(AnnotationDisplay, M_OVERLAY, M_ENABLE);
   MdispControl(AnnotationDisplay, M_OVERLAY_CLEAR, M_DEFAULT);
   MdispInquire(AnnotationDisplay, M_OVERLAY_ID, &AnnotationImage);
   MbufInquire(AnnotationImage, M_SIZE_X, &BufSizeX);
   MbufInquire(AnnotationImage, M_SIZE_Y, &BufSizeY);
   CursorColor = CURSOR_COLOR;
   MgraColor(M_DEFAULT, CursorColor);

   /* Write annotations. */
   n = (BufSizeX/FOCUS_MAX_NB_POSITIONS);
   MgraLine(M_DEFAULT, AnnotationImage, 0, CURSOR_POSITION+CURSOR_SIZE, 
                                        BufSizeX-1, CURSOR_POSITION+CURSOR_SIZE);
   MgraLine(M_DEFAULT, AnnotationImage, Position*n, CURSOR_POSITION+CURSOR_SIZE,
                                        Position*n-CURSOR_SIZE, CURSOR_POSITION);
   MgraLine(M_DEFAULT, AnnotationImage, Position*n, CURSOR_POSITION+CURSOR_SIZE, 
                                        Position*n+CURSOR_SIZE, CURSOR_POSITION);
   MgraLine(M_DEFAULT, AnnotationImage, Position*n-CURSOR_SIZE, CURSOR_POSITION, 
                                        Position*n+CURSOR_SIZE, CURSOR_POSITION);
}