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

#include "CookieDetection.h"

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

   MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
   MosPrintf(MIL_TEXT("This example demonstrates how to count cookies using 3d sheet-")
             MIL_TEXT("of-light\nprofiling. The system consists of two cameras and one ")
             MIL_TEXT("laser. Note\nthat during the setup of the grab, the cameras were ")
             MIL_TEXT("synchronized so\nthe same laser scan was provided to all cameras ")
             MIL_TEXT("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] = { 1200, 1200 };
   const MIL_DOUBLE CORNER_HINT_Y              [NUM_CAMERAS] = {  120,  120 };
   const MIL_DOUBLE OFFSET_Z                   [NUM_CAMERAS] = { -16.77, -16.77 };
   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] = { 4, 4 };
      const MIL_INT    CAL_PEAK_WIDTH_DELTA[NUM_CAMERAS] = { 3, 3 };
      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_laserline_h0.mim"), PLANE_Z[0][0] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laserline_h1.mim"), PLANE_Z[0][1] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laserline_h2.mim"), PLANE_Z[0][2] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laserline_h3.mim"), PLANE_Z[0][3] },
               { EX_PATH("Cam1RefPlanes\\Cam1_laserline_h4.mim"), PLANE_Z[0][4] }
            },
            { // second camera
               // RefImageName                                Zs
               { EX_PATH("Cam2RefPlanes\\Cam2_laserline_h0.mim"), PLANE_Z[1][0] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laserline_h1.mim"), PLANE_Z[1][1] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laserline_h2.mim"), PLANE_Z[1][2] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laserline_h3.mim"), PLANE_Z[1][3] },
               { EX_PATH("Cam2RefPlanes\\Cam2_laserline_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 = 2.0; // 3d Display FPS
         const MIL_INT    CAMERA_MAP_MIN_CONTRAST[]   = { 150, 150 };
         const MIL_INT    CAMERA_MAP_PEAK_WIDTH[]     = { 10 , 10 };
         const MIL_INT    CAMERA_MAP_PEAK_DELTA[]     = {  9 ,  9 };
         const MIL_DOUBLE CAMERA_MAP_SCAN_SPEED[]     = { 0.3158, 0.3158 };
         const MIL_DOUBLE CAMERA_MAX_FRAMES           = 1024;
         const MIL_DOUBLE CAMERA_DISPLACEMENT_MODE    = M_CURRENT;

         // Visualization volume information.
         SMapGeneration MapData;
         MapData.BoxCornerX       = - 10.00;
         MapData.BoxCornerY       =    0.00;
         MapData.BoxCornerZ       =    1.00;
         MapData.BoxSizeX         =  220.00;
         MapData.BoxSizeY         =  220.00;
         MapData.BoxSizeZ         = - 39.00;         
         MapData.MapSizeX         = 695;
         MapData.MapSizeY         = 695;
         MapData.PixelSizeX       = 0.317;
         MapData.PixelSizeY       = 0.317;
         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_cookies.avi"), 0,  0,  0,  0,  0 },
               { EX_PATH("Cam2_cookies.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 CookieBoxDepthmap = M_NULL;
         pExampleMngrFor3D->GenerateDepthMap(PointCloudContainer, SCAN_INFO.MapVisualizationData, &CookieBoxDepthmap);

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

         // 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(CookieBoxDepthmap != M_NULL)
            { MbufFree(CookieBoxDepthmap); }
         }
      }
   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 CCookieCounting::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_DARK_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 NUM_COOKIE_POSITIONS = 6;
   const MIL_INT COOKIE_RELATIVE_OFFSETS[NUM_COOKIE_POSITIONS][2] =
      {
         {  40,  42 },
         { 267,  49 },
         {  24, 230 },
         { 258, 224 },
         {  36, 424 },
         { 269, 419 }
      };

   const MIL_INT COOKIE_CHILD_SIZE[2] = { 75, 67 };
   const MIL_DOUBLE COOKIE_HEIGHT = -10.0;
   const MIL_INT EXPECTED_NUM_COOKIES = 3;

   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);

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

   // 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_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &MilRemapped8BitImage);
   MbufClear(MilRemapped8BitImage, 0);

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

   MgraClear(M_DEFAULT, MilGraphicList);

   // Find the cookie box.
   MmodFind(m_MilModel, MilRemapped8BitImage, m_MilModelResult);

   MIL_INT NumOfOccurences = 0;
   MIL_INT PositionX, PositionY;

   // Get information on the find.
   MmodGetResult(m_MilModelResult, M_DEFAULT, M_NUMBER + M_TYPE_MIL_INT, &NumOfOccurences);
   MmodControl(m_MilModelResult, M_DEFAULT, M_RESULT_OUTPUT_UNITS, M_PIXEL);

   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);
      MgraColor(MilGraphics, MOD_EDGE_COLOR);
      MmodDraw(MilGraphics, m_MilModelResult, MilGraphicList, M_DRAW_EDGES, M_ALL, M_DEFAULT);

      MIL_DOUBLE DeviationMean = 0.0;
      MIL_INT NumCookies = 0;
      MIL_TEXT_CHAR CookieString[MAX_STRING_LEN];
      MIL_TEXT_CHAR TempString[MAX_STRING_LEN];

      for (MIL_INT i = 0; i < NUM_COOKIE_POSITIONS; i++) 
         {
         // Create a child for each location of cookies in the box.
         MIL_ID CookieChild = 
         MbufChild2d(MilDepthMap, 
                     PositionX+COOKIE_RELATIVE_OFFSETS[i][0], 
                     PositionY+COOKIE_RELATIVE_OFFSETS[i][1],
                     COOKIE_CHILD_SIZE[0], COOKIE_CHILD_SIZE[1], 
                     M_NULL);

         // Calculate 3d statistics for each cookie location.
         M3dmapStat(CookieChild, M_NULL, M_NULL, M_NULL, M_DEVIATION_MEAN, M_DEFAULT, M_DEFAULT, &DeviationMean);

         // Determine the number of cookies according to the height.
         NumCookies = (MIL_INT)(DeviationMean/COOKIE_HEIGHT);

         if (NumCookies == EXPECTED_NUM_COOKIES)
            { MgraColor(MilGraphics, PROC_TEXT_PASS_COLOR); }
         else
            { MgraColor(MilGraphics, PROC_TEXT_FAIL_COLOR); }

         if (NumCookies == 1)
            { MosSprintf(TempString, MAX_STRING_LEN, MIL_TEXT("%s"), MIL_TEXT("")); }
         else
            { MosSprintf(TempString, MAX_STRING_LEN, MIL_TEXT("%s"), MIL_TEXT("s")); }


         MosSprintf(CookieString, MAX_STRING_LEN, MIL_TEXT("%d cookie%s"), NumCookies, TempString);

         MgraText(MilGraphics, MilGraphicList, 
                  PositionX+COOKIE_RELATIVE_OFFSETS[i][0]-10,
                  TEXT_OFFSET_Y+PositionY+COOKIE_RELATIVE_OFFSETS[i][1], 
                  CookieString);

         MbufFree(CookieChild);
         }

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

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

      MosPrintf(MIL_TEXT("The number of cookies in each location has been ")
                MIL_TEXT("calculated using the height.\n"));
      MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
      MosGetch();
      }
   else
      {
      // Enable graphics list update.
      MdispControl(MilDisplayMngr->GetDisplayID(), M_UPDATE_GRAPHIC_LIST, M_ENABLE);

      MosPrintf(MIL_TEXT("Could not find the cookie box.\n"));  
      MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
      MosGetch();
      }

   MbufFree(MilRemapped8BitImage);
   MilRemapped8BitImage = M_NULL;
   }

//*******************************************************************************
// Function that allocates processing objects.
//*******************************************************************************
void CCookieCounting::AllocProcessingObjects(MIL_ID MilSystem)
   {
   const MIL_TEXT_CHAR* COOKIE_BOX_MODEL = M_IMAGE_PATH MIL_TEXT("CookieDetection\\BoxModel.mmf");
   const MIL_TEXT_CHAR* DEPTH_MAP_CALIBRATION = M_IMAGE_PATH MIL_TEXT("CookieDetection\\DepthMapCalibration.cal");

   MmodAllocResult(MilSystem, M_DEFAULT, &m_MilModelResult);
   MmodRestore(COOKIE_BOX_MODEL, MilSystem, M_DEFAULT, &m_MilModel);

   // Associate the depth map calibration.
   McalRestore(DEPTH_MAP_CALIBRATION, MilSystem, M_DEFAULT, &m_MilDepthMapCalibration);
   MmodControl(m_MilModel, M_ALL, M_ASSOCIATED_CALIBRATION, m_MilDepthMapCalibration);

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

//*******************************************************************************
// Frees processing objects.
//*******************************************************************************
void CCookieCounting::FreeProcessingObjects()
   {
   MmodFree(m_MilModel);                  m_MilModel               = M_NULL;
   MmodFree(m_MilModelResult);            m_MilModelResult         = M_NULL;

   McalFree(m_MilDepthMapCalibration);    m_MilDepthMapCalibration = M_NULL;
   }