//***************************************************************************************/
// 
// File name: BaslerToF.cpp  
// Location: See Matrox Example Launcher in the MIL Control Center
// 
//
// Synopsis:  This program contains an example of 3d reconstruction by interfacing with
//            a Basler ToF camera. By default the example runs in standalone mode.
//            The camera output images have been saved on disk and will be reloaded.
//            See the PrintHeader() function below for a detailed description.
//
//            To run the example using an actual Basler ToF camera, the camera must be
//            connected to your computer or to a network and the USE_REAL_CAMERA defined at
//            the beginning of BaslerToF.cpp must be set to 1. You must also have installed
//            the necessary GenTL producer, provided by Basler, and consumer, provided by
//            Matrox Imaging. Refer to Basler to get the latest version of the producer.
//            Install MIL 10 update 34 to get the GenTL consumer.
//            
//            This example has been tested using the following :
//            -64 bits Windows OS
//            - Compiler : Microsoft Visual Studio 2010
//            - Basler ToF Driver : 1.0.1.1013
//            - Matrox Imaging Library : MIL 10.0 PP2 with Update 34
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2016.
// All Rights Reserved

#include <mil.h>
#include "BaslerToFExample.h"

#define USE_REAL_CAMERA 0

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

             MIL_TEXT("[SYNOPSIS]\n")
             MIL_TEXT("This example demonstrates how to acquire calibrated range data using \n")
             MIL_TEXT("a Basler ToF camera. The range data can either be a Range image or \n")
             MIL_TEXT("a calibrated point cloud. Once acquired, the range data is transformed or put \n")
             MIL_TEXT("into a MIL compatible 3d point cloud. The point cloud relative coordinate\n")
             MIL_TEXT("system is then positioned in the world on a learned flat horizontal surface.\n")
             MIL_TEXT("The point cloud is extracted in a depth map where a box is located.\n\n")
             
             MIL_TEXT("[MODULES USED]\n")
             MIL_TEXT("Modules used: 3dMap, application, buffer, calibration, display\n")
             MIL_TEXT("              image processing, system.\n\n"));
   }

//*****************************************************************************
// Standalone images.
//*****************************************************************************
#define EX_PATH(x) (M_IMAGE_PATH MIL_TEXT("BaslerToF/") MIL_TEXT(x))

static MIL_CONST_TEXT_PTR GRID_IMAGE = EX_PATH("Grid.mim");
static MIL_CONST_TEXT_PTR GRAB_IMAGES = EX_PATH("Objects.avi");
static MIL_CONST_TEXT_PTR PLANE_POINT_CLOUD_IMAGE = EX_PATH("PlanePointCloud.mim");
static MIL_CONST_TEXT_PTR BOX_POINT_CLOUD_IMAGE = EX_PATH("ObjectsPointCloud.avi");

//****************************************************************************
// Function prototypes.
//****************************************************************************
bool CheckForRequiredMILFile(MIL_CONST_TEXT_PTR FileName);

//*****************************************************************************
// Main.
//*****************************************************************************
int MosMain()
   {
   // Print Header.
   PrintHeader();

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

   // Check for required example files.
   if(!CheckForRequiredMILFile(GRID_IMAGE))
      {
      MappFree(MilApplication);
      return -1;
      }

   // Allocate the system and the digitizer.
   MIL_ID MilSystem = M_NULL;
   MIL_ID MilGridDig = M_NULL;
   MIL_ID MilBoxDig = M_NULL;
   MIL_ID MilPlanePointCloudDig = M_NULL;
   MIL_ID MilBoxPointCloudDig = M_NULL;

#if USE_REAL_CAMERA
   bool AllocationError = true;
   #ifdef M_GENTL
      // Try to allocate the GenTL system and digitizer.
      MappControl(M_ERROR, M_PRINT_DISABLE);
      MsysAlloc(M_SYSTEM_GENTL, M_DEFAULT, M_DEFAULT, &MilSystem);
      MdigAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_DEFAULT, &MilGridDig);
      AllocationError = MappGetError(M_CURRENT, M_NULL) != M_NULL_ERROR;
      MappControl(M_ERROR, M_PRINT_ENABLE);
      MilBoxDig = MilGridDig;
      MilPlanePointCloudDig = MilGridDig;
      MilBoxPointCloudDig = MilGridDig;
   #endif
   if (AllocationError)
      {
      MosPrintf(MIL_TEXT("To run the example using an actual Basler ToF camera, the camera  \n")
                MIL_TEXT("must be connected to your computer or to a network and you must have \n")
                MIL_TEXT("installed the necessary GenTL producer, provided by Basler,\n")
                MIL_TEXT("and consumer, provided by Matrox Imaging. Refer to Basler to get\n")
                MIL_TEXT("the latest version of the producer. Install MIL 10 update 34 to get\n")
                MIL_TEXT("the GenTL consumer.\n\n")
                MIL_TEXT("Press <Enter> to end.\n"));
      MosGetch();
      if (MilGridDig) MdigFree(MilGridDig);
      if (MilSystem) MsysFree(MilSystem);
      MappFree(MilApplication);
      return 0;
      }
#else
   MsysAlloc(M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, &MilSystem);
   MdigAlloc(MilSystem, M_DEFAULT, GRID_IMAGE, M_DEFAULT, &MilGridDig);
   MdigAlloc(MilSystem, M_DEFAULT, GRAB_IMAGES, M_DEFAULT, &MilBoxDig);
   MdigAlloc(MilSystem, M_DEFAULT, PLANE_POINT_CLOUD_IMAGE, M_DEFAULT, &MilPlanePointCloudDig);
   MdigAlloc(MilSystem, M_DEFAULT, BOX_POINT_CLOUD_IMAGE, M_DEFAULT, &MilBoxPointCloudDig);
#endif

   // Allocate the image example.
   CBaslerToFImageExample* pImageExample = new CBaslerToFImageExample(MilSystem, MilGridDig, MilBoxDig);

   // Print the example description.
   MosPrintf(MIL_TEXT("[RANGE IMAGE EXAMPLE]\n")
             MIL_TEXT("This example acquires a range image from the BaslerToF camera. The MIL\n")
             MIL_TEXT("calibration is used to transform the data into a point cloud whose \n")
             MIL_TEXT("relative coordinate system is put on the calibration grid.\n")
             MIL_TEXT("The example then proceeds to extract a depth map where a box is located.\n\n"));

   // Calibrate the camera.
   if (pImageExample->CalibrateCamera(MilGridDig))
      {
      // Locate boxes.
      pImageExample->LocateBoxes(MilBoxDig);
      }  

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

   // Delete the example.
   delete pImageExample;

   // Allocate the point cloud example.
   CBaslerToFPointCloudExample* pPointCloudExample = new CBaslerToFPointCloudExample(MilSystem, MilGridDig, MilPlanePointCloudDig);

   // Print the example description.
   MosPrintf(MIL_TEXT("[RANGE POINT CLOUD EXAMPLE]\n")
             MIL_TEXT("This example directly acquires a point cloud from the BaslerToF camera.\n")
             MIL_TEXT("A plane is then fitted on the depth map representation of the point cloud\n")
             MIL_TEXT("to position the relative coordinate system.\n")
             MIL_TEXT("The example then proceeds to extract a depth map where a box is located.\n\n"));

   // Learn the ground plane.
   if (pPointCloudExample->LearnPlane(MilPlanePointCloudDig))
      {
      // Locate boxes
      pPointCloudExample->LocateBoxes(MilBoxPointCloudDig);
      }

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

   delete pPointCloudExample;
      
   // Free MIL object
   if (MilBoxDig != MilGridDig)
      {
      MdigFree(MilBoxDig);
      MdigFree(MilPlanePointCloudDig);
      MdigFree(MilBoxPointCloudDig);
      }
   MdigFree(MilGridDig);

   MsysFree(MilSystem);
   MappFree(MilApplication);

   return 0;
   }

//****************************************************************************
// CheckForRequiredMILFile. Check for required files to run the example.    
//****************************************************************************
bool CheckForRequiredMILFile(MIL_CONST_TEXT_PTR FileName)
   {
   MIL_INT FilePresent;

   MappFileOperation(M_DEFAULT, FileName, M_NULL, M_NULL, M_FILE_EXISTS, M_DEFAULT, &FilePresent);
   if (FilePresent == M_NO)
      {
      MosPrintf(MIL_TEXT("\n")
                MIL_TEXT("The footage needed to run this example is missing. You need \n")
                MIL_TEXT("to obtain and apply a separate specific update to have it.\n\n"));

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

   return (FilePresent == M_YES);
   }