//*******************************************************************************
// 
// File name: BottleCapInspection.cpp  
// Location:  ...\Matrox Imaging\MILxxx\Examples\Processing\3dReconstruction\BottleCapInspection
//             \C++
//
// Synopsis: Demonstrates the inspection of bottle caps using 3d data.
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2015.
// All Rights Reserved

#include "BottleCapInspection.h"

//****************************************************************************
// Example description.
//****************************************************************************
void PrintHeader()   
   {
   MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n"));
   MosPrintf(MIL_TEXT("BottleCapInspection\n\n"));

   MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
   MosPrintf(MIL_TEXT("This example demonstrates the inspection of bottle caps using ") 
             MIL_TEXT("3d\nsheet-of-light profiling. The system consists of two ")
             MIL_TEXT("cameras and\none laser. Note that during the setup of the grab, ")
             MIL_TEXT("the cameras\nwere synchronized so the same laser scan was ")
             MIL_TEXT("provided to all\ncameras at the same time.\n"));
   MosPrintf(MIL_TEXT("\n\n"));

   MosPrintf(MIL_TEXT("[MODULES USED]\n"));
   MosPrintf(MIL_TEXT("Modules used: Application, system, display, buffer, ")
             MIL_TEXT("graphic, \nimage processing, calibration,"));
   MosPrintf(MIL_TEXT(" 3d reconstruction, model finder. \n"));
   }

//*****************************************************************************
// Main.
//*****************************************************************************
int MosMain(void)
   {
   PrintHeader();

   // Allocate the MIL application.
   MIL_ID MilApplication = MappAlloc(M_NULL, M_DEFAULT, M_NULL);

   // Initialization.
   CExampleManagerFor3D* pExampleMngrFor3D = MakeExampleManager();
   if (!pExampleMngrFor3D)
      {
      MappFree(MilApplication);
      return -1;
      }

   MosPrintf(MIL_TEXT("Press <Enter> to start.\n\n"));
   MosGetch();

   //.......................................................................
   // 1. To calibrate the setup, the first step is to calibrate the cameras.
   // Camera calibration specifications.
   const MIL_DOUBLE COL_SPACING              [NUM_CAMERAS] = { 8.83, 8.83 };
   const MIL_DOUBLE ROW_SPACING              [NUM_CAMERAS] = { 8.83, 8.83 };
   const MIL_INT NB_ROWS                     [NUM_CAMERAS] = { 22, 22 };
   const MIL_INT NB_COLS                     [NUM_CAMERAS] = { 18, 18 };
   const MIL_DOUBLE CORNER_HINT_X            [NUM_CAMERAS] = { 1000, 1500 };
   const MIL_DOUBLE CORNER_HINT_Y            [NUM_CAMERAS] = { 200, 100 };
   const MIL_DOUBLE OFFSET_Z                 [NUM_CAMERAS] = { 0, 0 };
   const MIL_INT64 CALIBRATION_TYPE          [NUM_CAMERAS] = { M_CHESSBOARD_GRID, M_CHESSBOARD_GRID };
   const MIL_CONST_TEXT_PTR GRID_IMG_FILENAME[NUM_CAMERAS] = { EX_PATH("Cam1_grid.mim"), EX_PATH("Cam2_grid.mim") };

   // Initialize data.
   SCameraCalibrationInfo CAMERA_CALIBRATION_INFO[NUM_CAMERAS];
   for (MIL_INT c = 0; c < NUM_CAMERAS; c++)
      {
      SCameraCalibrationInfo& CCI = CAMERA_CALIBRATION_INFO[c];
      CCI.CornerHintX          = CORNER_HINT_X[c];
      CCI.CornerHintY          = CORNER_HINT_Y[c];
      CCI.OffsetZ              = OFFSET_Z[c];
      CCI.NbRows               = NB_ROWS[c];
      CCI.NbCols               = NB_COLS[c];
      CCI.RowSpacing           = ROW_SPACING[c];
      CCI.ColSpacing           = COL_SPACING[c];
      CCI.CalibrationType      = CALIBRATION_TYPE[c];
      CCI.GridImageFilename    = GRID_IMG_FILENAME[c];
      CCI.Relocate             = NO_RELOCATE;
      CCI.RelocatedGridImageFilename = NULL;
      }

   //.................................
   // 1.1 Execute cameras calibration.
   MIL_ID CameraCalibrations[NUM_CAMERAS];
   bool CameraCalibrationOk = pExampleMngrFor3D->CalibrateCameras(CAMERA_CALIBRATION_INFO, NUM_CAMERAS, &CameraCalibrations[0]);

   //..................................................................
   // 2. Then continue to calibrate the laser planes (sheets-of-light).
   if(CameraCalibrationOk)
      {
      MosPrintf(MIL_TEXT("Press <Enter> to calibrate laser planes.\n\n"));
      MosGetch();

      // Sheet-of-light (laser plane) calibration      
      const MIL_INT    NUM_REF_PLANES                    = 5;
      const MIL_DOUBLE CAL_MIN_CONTRAST    [NUM_CAMERAS] = { 120, 120 };
      const MIL_INT    CAL_NB_REF_PLANES   [NUM_CAMERAS] = { NUM_REF_PLANES, NUM_REF_PLANES };
      const MIL_INT    CAL_SCAN_ORIENTATION[NUM_CAMERAS] = { M_HORIZONTAL, M_HORIZONTAL };
      const MIL_INT    CAL_PEAK_WIDTH      [NUM_CAMERAS] = {  8,  8 };
      const MIL_INT    CAL_PEAK_WIDTH_DELTA[NUM_CAMERAS] = {  7,  7 };
      const MIL_INT    LASER_LABELS        [NUM_CAMERAS] = { 1, 1 };
      const MIL_INT    CAMERA_LABELS       [NUM_CAMERAS] = { 1, 2 };

      const MIL_DOUBLE PLANE_Z[NUM_CAMERAS][MAX_NB_REF_PLANES] =   
         {  { 0.0, -5.86, -11.72, -17.58, -23.44 },
            { 0.0, -5.86, -11.72, -17.58, -23.44 } };

      const SRefPlaneInfo LASER_CALIBRATION_PLANES[NUM_CAMERAS][MAX_NB_REF_PLANES] = 
         { { // first camera
               // RefImageName                                Zs
               { EX_PATH("Cam1RefPlanes\\Cam1_laser_h0.mim"), PLANE_Z[0][0] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laser_h1.mim"), PLANE_Z[0][1] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laser_h2.mim"), PLANE_Z[0][2] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laser_h3.mim"), PLANE_Z[0][3] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laser_h4.mim"), PLANE_Z[0][4] }
            },
            { // second camera
               // RefImageName                                Zs
               { EX_PATH("Cam2RefPlanes\\Cam2_laser_h0.mim"), PLANE_Z[1][0] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laser_h1.mim"), PLANE_Z[1][1] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laser_h2.mim"), PLANE_Z[1][2] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laser_h3.mim"), PLANE_Z[1][3] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laser_h4.mim"), PLANE_Z[1][4] }
            } };

      const MIL_INT NUM_LASERS_PER_IMAGE = 1;

      SCameraLaserInfo LASER_CALIBRATION_INFO[NUM_CAMERAS * NUM_LASERS_PER_IMAGE];
      for(MIL_INT c = 0; c < NUM_CAMERAS; c++)
         {
         SCameraLaserInfo& LCI = LASER_CALIBRATION_INFO[c];

         LCI.NumLasersPerImage  = NUM_LASERS_PER_IMAGE;
         LCI.NumRefPlanes       = NUM_REF_PLANES;
         LCI.CalMinContrast     = CAL_MIN_CONTRAST[c];
         LCI.CalNbRefPlanes     = CAL_NB_REF_PLANES[c];
         LCI.CalScanOrientation = CAL_SCAN_ORIENTATION[c];
         LCI.CalPeakWidthNominal= CAL_PEAK_WIDTH[c];
         LCI.CalPeakWidthDelta  = CAL_PEAK_WIDTH_DELTA[c];
         for(MIL_INT l = 0; l < LCI.CalNbRefPlanes; l++)
            {
            LCI.LaserCalibrationPlanes[l] = LASER_CALIBRATION_PLANES[c][l];
            }
         LCI.LaserLabel  = LASER_LABELS[c];
         LCI.CameraLabel = CAMERA_LABELS[c];
         LCI.LineExtractionInROI = eLineNoROI;
         }

      //............................................................
      // 2.1 Execute the calibration of the laser planes.
      // Generates the needed calibrated camera-laser pair contexts.
      MIL_ID CameraLaserCtxts[NUM_CAMERAS * NUM_LASERS_PER_IMAGE];
      bool SheetOfLightOk = pExampleMngrFor3D->CalibrateSheetOfLight(&LASER_CALIBRATION_INFO[0],
                                                                     &CameraCalibrations[0],
                                                                     &CameraLaserCtxts[0]);
      if (SheetOfLightOk)
         {
         // Map generation specifications.
         const MIL_DOUBLE D3D_DISPLAY_REFRESH_RATE    = 1.0; // 3d Display FPS
         const MIL_INT    CAMERA_MAP_MIN_CONTRAST[]   = { 120, 120 };
         const MIL_INT    CAMERA_MAP_PEAK_WIDTH[]     = {  8,  8 };
         const MIL_INT    CAMERA_MAP_PEAK_DELTA[]     = {  7,  7 };
         const MIL_DOUBLE CAMERA_MAP_SCAN_SPEED[]     = { 0.2697, 0.2697 };
         const MIL_DOUBLE CAMERA_MAX_FRAMES           = 1196;
         const MIL_DOUBLE CAMERA_DISPLACEMENT_MODE    = M_CURRENT;

         // Visualization volume information.
         SMapGeneration MapData;
         MapData.BoxCornerX       = - 25.00;
         MapData.BoxCornerY       =    8.00;
         MapData.BoxCornerZ       =   24.00;
         MapData.BoxSizeX         =  220.00;
         MapData.BoxSizeY         =  266.00;
         MapData.BoxSizeZ         = - 30.00;
         MapData.MapSizeX         = 830;
         MapData.MapSizeY         = 1020;
         MapData.PixelSizeX       = MapData.BoxSizeX / (MapData.MapSizeX - 1.0);
         MapData.PixelSizeY       = MapData.BoxSizeY / (MapData.MapSizeY - 1.0);
         MapData.GrayScaleZ       = MapData.BoxSizeZ / 65534.0;
         MapData.IntensityMapType = 8 + M_UNSIGNED;
         MapData.SetExtractOverlap= true;
         MapData.ExtractOverlap   = M_MAX;
         MapData.FillXThreshold   = 1.0;
         MapData.FillYThreshold   = 1.0;

         // Scan and analyze information.
         SPointCloudAcquisitionInfo SCAN_INFO =
            {
            // SD3DSysInfo
            { D3D_DISPLAY_REFRESH_RATE, SHOW_COLOR },
            { CAMERA_MAP_MIN_CONTRAST[0] , CAMERA_MAP_MIN_CONTRAST[1] },
            { CAMERA_MAP_PEAK_WIDTH[0]   , CAMERA_MAP_PEAK_WIDTH[1]   },
            { CAMERA_MAP_PEAK_DELTA[0]   , CAMERA_MAP_PEAK_DELTA[1]   },
            { CAMERA_MAP_SCAN_SPEED[0]   , CAMERA_MAP_SCAN_SPEED[1]   },
            CAMERA_MAX_FRAMES,
            CAMERA_DISPLACEMENT_MODE,
            eLineNoROI,
            // SLineExtractionInROI
            { 0, 0, 0, 0 },
            MapData,
            {  // DigInfo
               // DigFormat                  SX  SY  SB Type NbFrames
               { EX_PATH("Cam1_bottles.avi"), 0,  0,  0,  0,  0 },
               { EX_PATH("Cam2_bottles.avi"), 0,  0,  0,  0,  0 }
            },
            // ScanDisplayText
            MIL_TEXT("Color legend:\n \tGray\t  \t= missing data\n \tDark blue \t= ")
            MIL_TEXT("minimum height\n \tGreen, Yellow \t= middle height\n \tDark ")
            MIL_TEXT("red  \t= maximum height\n")
            MIL_TEXT("\n")
            };

         // Update some information from the sequences on disk.
         for(MIL_INT d = 0; d < NUM_CAMERAS; d++)
            { SCAN_INFO.DigInfo[d].UpdateInfoFromDisk(); }

         //....................................................
         // 3. Acquire a 3d point cloud by scanning the object.
         //    The point cloud container will hold one point cloud per camera-laser pair.
         MIL_ID PointCloudContainer = M_NULL;
         bool PointCloudOk = pExampleMngrFor3D->AcquirePointCloud(eScan, &SCAN_INFO, CameraLaserCtxts, &PointCloudContainer);

         //.....................................................................................
         // 4. Generate the depth map (orthogonal 2d-projection) of the acquired 3d point cloud.
         MIL_ID BottleCapsDepthmap = M_NULL;         
         pExampleMngrFor3D->GenerateDepthMap(PointCloudContainer, SCAN_INFO.MapVisualizationData, &BottleCapsDepthmap);

         //....................................
         // 5. Analyze the generated depth map.
         CAnalyzeBottleCap ProbObj;
         pExampleMngrFor3D->AnalyzeDepthMap(&ProbObj, BottleCapsDepthmap);

         // Free camera-laser contexts.
         for (MIL_INT c = 0; c < NUM_CAMERAS; c++)
            {
            for (MIL_INT l = 0; l < NUM_LASERS_PER_IMAGE; l++)
               {
               MIL_ID& CameraLaserCtx = CameraLaserCtxts[(c*NUM_LASERS_PER_IMAGE) + l];
               if (CameraLaserCtx != M_NULL)
                  {
                  M3dmapFree(CameraLaserCtx);
                  CameraLaserCtx = M_NULL;
                  }
               }
            }

         M3dmapFree(PointCloudContainer);
         if(BottleCapsDepthmap != M_NULL)
            { MbufFree(BottleCapsDepthmap); }
         };
      }
   else
      {
      // A problem occurred calibrating the cameras.
      MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
      MosGetch();
      }

   // Free camera calibrations.
   for (MIL_INT c = 0; c < NUM_CAMERAS; c++)
      {
      if(CameraCalibrations[c] != M_NULL)
         {
         McalFree(CameraCalibrations[c]);
         CameraCalibrations[c] = M_NULL;
         }
      }

   delete pExampleMngrFor3D;
   pExampleMngrFor3D = NULL;

   // Free the MIL application.
   MappFree(MilApplication);

   return 0;
   }

//*******************************************************************************
// Function that analyzes the scanned object.
//*******************************************************************************
void CAnalyzeBottleCap::Analyze(SCommonAnalysisObjects& CommonAnalysisObjects)
   {
   // Processing display zoom factor
   const MIL_DOUBLE PROC_DISPLAY_ZOOM_FACTOR_X = 0.8;
   const MIL_DOUBLE PROC_DISPLAY_ZOOM_FACTOR_Y = 0.8;

   // Color specifications
   const MIL_DOUBLE PROC_PASS_COLOR = M_COLOR_GREEN;
   const MIL_DOUBLE PROC_FAIL_COLOR = M_COLOR_RED;

   const MIL_INT    CAP_DELTA_X          = 40;
   const MIL_INT    CAP_DELTA_Y          = 40;
   const MIL_INT    MAX_CAP_MISSING_DATA = 1000;

   const MIL_INT    PLANE_DELTA_X        = 40;
   const MIL_INT    PLANE_DELTA_Y        = 40;
   const MIL_INT    PLANE_SIZE_X         = PLANE_DELTA_X*2;
   const MIL_INT    PLANE_SIZE_Y         = PLANE_DELTA_Y*2;
   const MIL_DOUBLE ANGLE_TOLERANCE_DEG  = 4.0;
   const MIL_DOUBLE HEIGHT_TOLERANCE     = 2.0;   

   MIL_ID MilSystem            = CommonAnalysisObjects.MilSystem;
   MIL_ID MilGraphics          = CommonAnalysisObjects.MilGraphics;
   MIL_ID MilGraphicList       = CommonAnalysisObjects.MilGraphicList;
   MIL_ID MilDepthMap          = CommonAnalysisObjects.MilDepthMap;
   MIL_ID MilGeometry          = m_Geometry;
   MIL_ID MilReferenceGeometry = m_ReferenceGeometry;
   CMILDisplayManager* DispMngr = CommonAnalysisObjects.MilDisplays;
   MIL_DOUBLE AverageHeight = 0.0;

   // Allocate a mask image for plane fit.
   MIL_ID MilMaskImage = 
   MbufAlloc2d(MilSystem, MbufInquire(MilDepthMap, M_SIZE_X, M_NULL),
               MbufInquire(MilDepthMap, M_SIZE_Y, M_NULL),
               MbufInquire(MilDepthMap, M_TYPE, M_NULL),
               M_IMAGE + M_PROC, M_NULL);

   // Disable graphics list update.
   MdispControl(DispMngr->GetDisplayID(), M_UPDATE_GRAPHIC_LIST, M_DISABLE);

   // Set 0's to invalid data.
   MbufClearCond(MilDepthMap, 65535, 65535, 65535, MilDepthMap, M_EQUAL, 0);

   // Setup the display.
   MgraClear(M_DEFAULT, MilGraphicList);
   DispMngr->Zoom(PROC_DISPLAY_ZOOM_FACTOR_X, PROC_DISPLAY_ZOOM_FACTOR_Y);

   // Allocate the necessary buffers for processing.
   MIL_ID MilRemapped8BitImage;
   MbufAlloc2d(MilSystem,
               MbufInquire(MilDepthMap, M_SIZE_X, M_NULL),
               MbufInquire(MilDepthMap, M_SIZE_Y, M_NULL),
               8, M_IMAGE + M_PROC + M_DISP, &MilRemapped8BitImage);

   MbufClear(MilRemapped8BitImage, 0);

   // Remap 16-bit depth map to 8 bit.
   MimShift(MilDepthMap, MilRemapped8BitImage, -8);

   MgraClear(M_DEFAULT, MilGraphicList);

   // Disassociate the calibration from the binarized image because we will not use it.
   McalAssociate(M_NULL, MilRemapped8BitImage, M_DEFAULT);

   // Find the bottle caps.
   MmodFind(m_CapModel, MilRemapped8BitImage, m_CapModelResult);

   MIL_INT NumOfOccurrences = 0;
   MIL_INT *PositionX, *PositionY;

   // Get information on the find.
   MmodGetResult(m_CapModelResult, M_DEFAULT, M_NUMBER + M_TYPE_MIL_INT, &NumOfOccurrences);

   PositionX = new MIL_INT [NumOfOccurrences];
   PositionY = new MIL_INT [NumOfOccurrences];

   MmodGetResult(m_CapModelResult, M_DEFAULT, M_POSITION_X + M_TYPE_MIL_INT, PositionX);
   MmodGetResult(m_CapModelResult, M_DEFAULT, M_POSITION_Y + M_TYPE_MIL_INT, PositionY);

   SortCapPositions(PositionX, PositionY, NumOfOccurrences);

   if (NumOfOccurrences > 0)
      {
      CAnalyzeBottleCap::SResults* BottleResults = new CAnalyzeBottleCap::SResults[NumOfOccurrences];

      // Check measurements on each bottle cap location.
      for (MIL_INT i = 0; i < NumOfOccurrences; i++)
         {
         MIL_INT PosX, PosY;
         MIL_TEXT_CHAR OccIdxStr[MAX_STRING_LEN];
         MosSprintf(OccIdxStr, MAX_STRING_LEN, MIL_TEXT("%2d"), (int) i);

         PosX = PositionX[i] - CAP_DELTA_X;
         PosY = PositionY[i] - CAP_DELTA_Y;

         MIL_ID CapChild;
         MIL_DOUBLE MissingData = 0.0;
         MbufChild2d(MilDepthMap, 
                     PosX, 
                     PosY,
                     CAP_DELTA_X*2, CAP_DELTA_Y*2, 
                     &CapChild);

         // Check if the bottle is open by looking for missing data.
         M3dmapStat(CapChild, M_NULL, M_NULL, M_NULL,
                    M_NUMBER_OF_PIXELS_MISSING_DATA , M_DEFAULT, M_DEFAULT, &MissingData);

         MosSprintf(BottleResults[i].MissingData, MAX_STRING_LEN, MIL_TEXT("%.0f"), MissingData);

         if (MissingData > MAX_CAP_MISSING_DATA)
            {
            MosSprintf(BottleResults[i].Status, MAX_STRING_LEN, MIL_TEXT("open"));
            MosSprintf(BottleResults[i].Angle, MAX_STRING_LEN, MIL_TEXT("n/a"));
            MosSprintf(BottleResults[i].MeanDeviation, MAX_STRING_LEN, MIL_TEXT("n/a"));

            MgraColor(MilGraphics, PROC_FAIL_COLOR);

            MgraText(MilGraphics, MilGraphicList, PosX+10, PosY+20, MIL_TEXT("open"));
            }
         else
            {
            // Clear the mask image.
            MbufClear(MilMaskImage, 0);

            // Create a child for location of plane fit.
            MIL_ID MilMaskChild =
            MbufChild2d(MilMaskImage,
                        PositionX[i] - PLANE_DELTA_X,
                        PositionY[i] - PLANE_DELTA_Y, 
                        PLANE_SIZE_X,
                        PLANE_SIZE_Y,
                        M_NULL);
            MbufClear(MilMaskChild, 65535);

            const MIL_DOUBLE FIT_OUTLIER_DISTANCE = 2.0;
            // Define the plane Ax + By + Z0 = D using the mask.
            M3dmapSetGeometry(MilGeometry, M_PLANE, M_FIT, (MIL_DOUBLE) MilDepthMap, 
                              (MIL_DOUBLE) MilMaskImage, FIT_OUTLIER_DISTANCE, M_DEFAULT, M_DEFAULT);

            if(M3dmapInquire(MilGeometry, M_DEFAULT, M_STATUS, M_NULL) == M_SUCCESS)
               {
               MIL_DOUBLE A, B, C;

               // Get the plane coefficients.
               M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_AX, &A);
               M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_AY, &B);
               C = -1.0; // by definition of z(x,y) = Z0 + AX*x + Ay*y

               // Calculate the dot product between ref plane and cap plane 
               // assuming the plane is horizontal with normal (0, 0, -1).
               MIL_DOUBLE PlaneDotProduct = -C;

               // Get the length of the vectors.
               MIL_DOUBLE RefVectorLength = 1.0;  // Length of (0, 0, -1)
               MIL_DOUBLE CapVectorLength = sqrt(A*A + B*B + C*C);

               // Calculate the angle between the reference plane and the cap plane.
               MIL_DOUBLE AngleRad = acos(PlaneDotProduct / (RefVectorLength * CapVectorLength));
               MIL_DOUBLE AngleDeg = (AngleRad * 180.0) / 3.14159;
               MosSprintf(BottleResults[i].Angle, MAX_STRING_LEN, MIL_TEXT("%.2f"), AngleDeg);

               if (AngleDeg < ANGLE_TOLERANCE_DEG)
                  {
                  // Check the elevation relative to the reference plane.
                  M3dmapStat(CapChild, MilReferenceGeometry, M_NULL, M_NULL, 
                             M_DEVIATION_MEAN + M_STAT_ALL, M_INFINITE, M_DEFAULT, &AverageHeight);
                  MosSprintf(BottleResults[i].MeanDeviation, MAX_STRING_LEN, MIL_TEXT("%.2f"), AverageHeight);

                  if (AverageHeight > HEIGHT_TOLERANCE)
                     {
                     MosSprintf(BottleResults[i].Status, MAX_STRING_LEN, MIL_TEXT("elevated"));
                     MgraColor(MilGraphics, PROC_FAIL_COLOR);
                     MgraText(MilGraphics, MilGraphicList, PosX-5, PosY+20, MIL_TEXT("elevated"));
                     }
                  else
                     {
                     MosSprintf(BottleResults[i].Status, MAX_STRING_LEN, MIL_TEXT("pass"));
                     MgraColor(MilGraphics, PROC_PASS_COLOR);
                     MgraRect(MilGraphics, MilGraphicList,PosX, PosY, PosX+CAP_DELTA_X*2, PosY+CAP_DELTA_Y*2);
                     }
                  }
               else
                  {
                  MosSprintf(BottleResults[i].Status, MAX_STRING_LEN, MIL_TEXT("tilted"));
                  MosSprintf(BottleResults[i].MeanDeviation, MAX_STRING_LEN, MIL_TEXT("n/a"));

                  MgraColor(MilGraphics, PROC_FAIL_COLOR);
                  MgraText(MilGraphics, MilGraphicList, PosX+10, PosY+20, MIL_TEXT("tilted"));
                  }
               }

            MbufFree(MilMaskChild);
            }

         // Draw the occurrence number in the current color.
         MgraText(MilGraphics, MilGraphicList, PosX-50, PosY-50, OccIdxStr);
         MbufFree(CapChild);
         }

      // Enable graphics list update.
      MdispControl(DispMngr->GetDisplayID(), M_UPDATE_GRAPHIC_LIST, M_ENABLE);

      // Show the result.
      DispMngr->Show(MilRemapped8BitImage);

      MosPrintf(MIL_TEXT("The bottle caps have been extracted and the inspection ")
                MIL_TEXT("results are displayed.\nFor each cap that was found, its ")
                MIL_TEXT("inclination was verified relative to a \nknown reference ")
                MIL_TEXT("bottle cap to determine whether it was tilted.\n\n"));

      MosPrintf(MIL_TEXT("---------------------------------------------------------------\n"));
      MosPrintf(MIL_TEXT("Index   Missing Data   Angle Deg.  Mean Deviation      Status  \n"));
      MosPrintf(MIL_TEXT("---------------------------------------------------------------\n"));

      for (MIL_INT i = 0; i < NumOfOccurrences; i++)
         {
         MosPrintf(MIL_TEXT("  %-2d     %-12s    %-5s     %10s           %-1s\n"), i, 
            BottleResults[i].MissingData, BottleResults[i].Angle,
            BottleResults[i].MeanDeviation, BottleResults[i].Status);
         }

      MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
      MosGetch();

      delete [] BottleResults;
      }
   else
      {
      MosPrintf(MIL_TEXT("Error: No bottle caps were found.\n\n"));
      }

   delete [] PositionX;
   delete [] PositionY;

   MbufFree(MilMaskImage);
   MbufFree(MilRemapped8BitImage);
   }

//*******************************************************************************
// Function that allocates processing objects.
//*******************************************************************************
void CAnalyzeBottleCap::AllocProcessingObjects(MIL_ID MilSystem)
   {
   const MIL_TEXT_CHAR* CAP_MODEL        = EX_PATH("CapModel.mmf");
   const MIL_DOUBLE CAP_REF_PLANE_HEIGHT = 4.0;

   // Restore and setup the cap model.
   MmodAllocResult(MilSystem, M_DEFAULT, &m_CapModelResult);
   MmodRestore(CAP_MODEL, MilSystem, M_DEFAULT, &m_CapModel);

   // Preprocess the model.
   MmodPreprocess(m_CapModel, M_DEFAULT);

   // Allocate a geometry object.
   M3dmapAlloc(MilSystem, M_GEOMETRY, M_DEFAULT, &m_Geometry);

   // Allocate a geometry object to use as the caps reference plane.
   M3dmapAlloc(MilSystem, M_GEOMETRY, M_DEFAULT, &m_ReferenceGeometry);
   M3dmapSetGeometry(m_ReferenceGeometry, M_HORIZONTAL_PLANE, M_PARAMETRIC, CAP_REF_PLANE_HEIGHT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT);
   }

//
//*******************************************************************************
// Function that frees processing objects.
//*******************************************************************************
void CAnalyzeBottleCap::FreeProcessingObjects()
   {
   MmodFree(m_CapModel); m_CapModel = M_NULL;
   MmodFree(m_CapModelResult); m_CapModelResult = M_NULL;

   M3dmapFree(m_Geometry); m_Geometry = M_NULL;
   M3dmapFree(m_ReferenceGeometry); m_ReferenceGeometry = M_NULL;
   }

//*******************************************************************************
// Function to sort the found cap positions.
//*******************************************************************************
void CAnalyzeBottleCap::SortCapPositions(MIL_INT* pX, MIL_INT* pY, MIL_INT Nb)
   {
   const MIL_INT RowMaxYDeviation = 80;

   // Use a simple non-optimal sort for the sake of simplicity.
   // Sort in Y then X.
   for(MIL_INT i = 0; i < Nb; i++)
      {
      for(MIL_INT j = 0; j < Nb; j++)
         {
         bool SwapPos = false;

         MIL_INT DeltaY = (pY[i] - pY[j]);
         if(abs((MIL_INT32)DeltaY) <= RowMaxYDeviation) // Same row
            {
            MIL_INT DeltaX = pX[i] - pX[j]; // column discriminate
            SwapPos = (DeltaX < 0);
            }
         else
            {
            SwapPos = (DeltaY < 0); // row discriminate
            }

         if(SwapPos)
            {
            std::swap(pX[i], pX[j]);
            std::swap(pY[i], pY[j]);
            }
         }
      }
   }