//*******************************************************************************
// 
// File name: BlisterPackInspection.cpp  
// Location:  ...\Matrox Imaging\MILxxx\Examples\Processing\3dReconstruction
//             \BlisterPackInspection\C++
//
// Synopsis: Demonstrates the inspection of a blister pack using 3d
//           sheet-of-light profiling.
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2015.
// All Rights Reserved

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

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

   MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
   MosPrintf(MIL_TEXT("This example demonstrates the inspection of a blister pack ")
             MIL_TEXT("using 3d\nsheet-of-light profiling. The system consists of ")
             MIL_TEXT("two cameras and\none laser.  Note that during the setup ")
             MIL_TEXT("of the grab, the cameras were\nsynchronized so the same ")
             MIL_TEXT("laser scan was provided to all cameras\nat the same ")
             MIL_TEXT("time.\n"));
   MosPrintf(MIL_TEXT("\n\n"));

   MosPrintf(MIL_TEXT("[MODULES USED]\n"));
   MosPrintf(MIL_TEXT("Modules used: Application, system, display, buffer, "));
   MosPrintf(MIL_TEXT("graphic, \nimage processing, calibration, ")
             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] = { 5.0, 5.0 };
   const MIL_DOUBLE ROW_SPACING                [NUM_CAMERAS] = { 5.0, 5.0 };
   const MIL_INT    NB_ROWS                    [NUM_CAMERAS] = { 16, 16 };
   const MIL_INT    NB_COLS                    [NUM_CAMERAS] = { 15, 15 };
   const MIL_DOUBLE CORNER_HINT_X              [NUM_CAMERAS] = { 940, 1210 };
   const MIL_DOUBLE CORNER_HINT_Y              [NUM_CAMERAS] = { 260, 155 };
   const MIL_DOUBLE OFFSET_Z                   [NUM_CAMERAS] = { -5.05, -5.05 };
   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] = { 150, 150 };
      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] = { 3, 3 };
      const MIL_INT    CAL_PEAK_WIDTH_DELTA[NUM_CAMERAS] = { 2, 2 };
      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] =
         {  { -5.05, -10.91, -16.77, -22.63, -28.49 },
            { -5.05, -10.91, -16.77, -22.63, -28.49 } };

      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_PER_SEC = 3.0; // 3d Display FPS
         const MIL_INT    CAMERA_MAP_MIN_CONTRAST[]   = { 150, 150 };
         const MIL_INT    CAMERA_MAP_PEAK_WIDTH[]     = { 25 , 25 };
         const MIL_INT    CAMERA_MAP_PEAK_DELTA[]     = { 30 , 30 };
         const MIL_DOUBLE CAMERA_MAP_SCAN_SPEED[]     = { 0.28235, 0.28235 };
         const MIL_DOUBLE CAMERA_MAX_FRAMES           = 1024;
         const MIL_DOUBLE CAMERA_DISPLACEMENT_MODE    = M_CURRENT;

         // Visualization volume information.
         SMapGeneration MapData;
         MapData.BoxCornerX       = - 20.00;
         MapData.BoxCornerY       =    0.00;
         MapData.BoxCornerZ       =    0.50;
         MapData.BoxSizeX         =  112.00;
         MapData.BoxSizeY         =  112.00;
         MapData.BoxSizeZ         = -  8.00;         
         MapData.MapSizeX         = 400;
         MapData.MapSizeY         = 400;
         MapData.PixelSizeX       = 0.22;
         MapData.PixelSizeY       = 0.22;
         MapData.GrayScaleZ       = (MapData.BoxSizeZ / 65534.0);
         MapData.IntensityMapType = 8 + M_UNSIGNED;
         MapData.SetExtractOverlap= true;
         MapData.ExtractOverlap   = M_MIN;
         MapData.FillXThreshold   = 1.0;
         MapData.FillYThreshold   = 1.0;

         // Scan and analyze information.
         SPointCloudAcquisitionInfo SCAN_INFO =
            {
            // SD3DSysInfo
            { D3D_DISPLAY_REFRESH_PER_SEC, SHOW_NO_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_blister_pack.avi"), 0,  0,  0,  0,  0 },
               { EX_PATH("Cam2_blister_pack.avi"), 0,  0,  0,  0,  0 }
            },
            MIL_TEXT("") // ScanDisplayText
            };

         // 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 BlisterPackDepthmap = M_NULL;
         pExampleMngrFor3D->GenerateDepthMap(PointCloudContainer, SCAN_INFO.MapVisualizationData, &BlisterPackDepthmap);

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

         // 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(BlisterPackDepthmap != M_NULL)
            { MbufFree(BlisterPackDepthmap); }
         }
      }
   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 CAnalyzeBlisterPack::Analyze(SCommonAnalysisObjects& CommonAnalysisObjects)
   {
   // Processing display zoom factor.
   const MIL_DOUBLE PROC_DISPLAY_ZOOM_FACTOR_X = 1;
   const MIL_DOUBLE PROC_DISPLAY_ZOOM_FACTOR_Y = 1;

   // Color specifications
   const MIL_DOUBLE PROC_TEXT_PASS_COLOR     = M_COLOR_GREEN;
   const MIL_DOUBLE PROC_TEXT_FAIL_COLOR     = M_COLOR_RED;
   const MIL_DOUBLE MOD_BOX_COLOR            = M_COLOR_CYAN;
   const MIL_DOUBLE MOD_EDGE_COLOR           = M_COLOR_GREEN;

   const MIL_INT        ITEM_CHILD_SIZE_X   = 30;
   const MIL_INT        ITEM_CHILD_SIZE_Y   = 30;
   const MIL_INT        ITEM_DISTANCE_X     = 70;
   const MIL_INT        ITEM_DISTANCE_Y     = 72;
   const MIL_INT        NUM_ITEMS_X         = 3;
   const MIL_INT        NUM_ITEMS_Y         = 4;
   const MIL_DOUBLE     ITEM_HEIGHT         = -5.0;

   MIL_ID MilSystem        = CommonAnalysisObjects.MilSystem;
   MIL_ID MilGraphics      = CommonAnalysisObjects.MilGraphics;
   MIL_ID MilGraphicList   = CommonAnalysisObjects.MilGraphicList;
   MIL_ID MilDepthMap      = CommonAnalysisObjects.MilDepthMap;
   CMILDisplayManager* MilDisplayMngr = CommonAnalysisObjects.MilDisplays;

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

   // Setup the display.
   MgraClear(M_DEFAULT, MilGraphicList);
   MilDisplayMngr->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);

   // Remove the associated calibration coming from the 16-bit depth map.
   McalAssociate(M_NULL, MilRemapped8BitImage, M_DEFAULT);

   // Find the blister pack.
   MmodFind(m_MilModel, MilRemapped8BitImage, m_MilModelResult);

   MIL_INT NumOfOccurences = 0;
   MIL_INT PositionX = -1, PositionY = -1;
   MmodGetResult(m_MilModelResult, M_DEFAULT, M_NUMBER + M_TYPE_MIL_INT, &NumOfOccurences);
   MmodGetResult(m_MilModelResult, M_DEFAULT, M_POSITION_X + M_TYPE_MIL_INT, &PositionX);
   MmodGetResult(m_MilModelResult, M_DEFAULT, M_POSITION_Y + M_TYPE_MIL_INT, &PositionY);

   if (NumOfOccurences >= 1)
      {
      // Draw the box if found.
      MgraControl(MilGraphics, M_BACKGROUND_MODE, M_OPAQUE);
      MgraControl(MilGraphics, M_FONT_SIZE, TEXT_FONT_SIZE_MEDIUM);

      MgraControl(MilGraphics, M_BACKGROUND_MODE, M_TRANSPARENT);         
      MgraColor(MilGraphics, MOD_BOX_COLOR);
      MmodDraw(MilGraphics, m_MilModelResult, MilGraphicList, M_DRAW_BOX, M_ALL, M_DEFAULT);

      MIL_ID ItemChild;
      MIL_DOUBLE DeviationMean = 0.0;
      MIL_INT OffsetX, OffsetY;
      for (MIL_INT y = 0; y < NUM_ITEMS_Y; y++)
         {
         for (MIL_INT x = 0; x < NUM_ITEMS_X; x++)
            {
            OffsetX = PositionX + x*ITEM_DISTANCE_X;
            OffsetY = PositionY + y*ITEM_DISTANCE_Y;

            MbufChild2d(MilDepthMap,
                        OffsetX, OffsetY,
                        ITEM_CHILD_SIZE_X, ITEM_CHILD_SIZE_Y,
                        &ItemChild);

            M3dmapStat(ItemChild, M_NULL, M_NULL, M_NULL, M_DEVIATION_MEAN, M_DEFAULT, M_DEFAULT, &DeviationMean);

            if (DeviationMean > ITEM_HEIGHT)
               {
               // Fail.
               MgraColor(MilGraphics, PROC_TEXT_FAIL_COLOR);
               MgraLine(MilGraphics, MilGraphicList, OffsetX, OffsetY, OffsetX+ITEM_CHILD_SIZE_X, OffsetY+ITEM_CHILD_SIZE_Y);
               MgraLine(MilGraphics, MilGraphicList, OffsetX+ITEM_CHILD_SIZE_X, OffsetY, OffsetX, OffsetY+ITEM_CHILD_SIZE_Y);
               MgraRect(MilGraphics, MilGraphicList, OffsetX, OffsetY, OffsetX+ITEM_CHILD_SIZE_X, OffsetY+ITEM_CHILD_SIZE_Y);
               }
            else
               {
               // Pass.
               MgraColor(MilGraphics, PROC_TEXT_PASS_COLOR);
               MgraRect(MilGraphics, MilGraphicList, OffsetX, OffsetY, OffsetX+ITEM_CHILD_SIZE_X, OffsetY+ITEM_CHILD_SIZE_Y);
               }

            MbufFree(ItemChild);
            }
         }
      
      MosPrintf(MIL_TEXT("The blister pack items were verified and the results"));
      MosPrintf(MIL_TEXT(" are shown.\n\tGreen box: Pass\n\tRed box: Missing or ")
                MIL_TEXT("broken\n\n"));
      }
   else
      {
      MosPrintf(MIL_TEXT("Blister pack not found.\n"));
      }

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

   MilDisplayMngr->Show(MilRemapped8BitImage);

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

   MbufFree(MilRemapped8BitImage);
   }

//*******************************************************************************
// Function that allocates processing objects.
//*******************************************************************************
void CAnalyzeBlisterPack::AllocProcessingObjects(MIL_ID MilSystem)
   {
   const MIL_TEXT_CHAR* PACK_MODEL = EX_PATH("PackModel.mmf"); 

   // Allocate objects to locate the blister pack.
   MmodAllocResult(MilSystem, M_DEFAULT, &m_MilModelResult);
   MmodRestore(PACK_MODEL, MilSystem, M_DEFAULT, &m_MilModel);

   // Preprocess the model.
   MmodPreprocess(m_MilModel, M_DEFAULT);
   }

//*******************************************************************************
// Function that frees processing objects.
//*******************************************************************************
void CAnalyzeBlisterPack::FreeProcessingObjects()
   {
   if(m_MilModelResult != M_NULL)
      { MmodFree(m_MilModelResult); m_MilModelResult = M_NULL; }

   if(m_MilModel != M_NULL)
      { MmodFree(m_MilModel); m_MilModel = M_NULL; }
   }