//***************************************************************************************
// 
// File name: 3dPlaneFit.cpp  
// Location: See Matrox Example Launcher in the MIL Control Center
// 
//
// Synopsis: This program contains an example of a 3d plane fit using the 3d 
//           reconstruction module.
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2016.
// All Rights Reserved

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

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

   MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
   MosPrintf(MIL_TEXT("This example demonstrates the definition and usage of "));
   MosPrintf(MIL_TEXT("a 3d plane fit.\n\n"));

   MosPrintf(MIL_TEXT("[MODULES USED]\n"));
   MosPrintf(MIL_TEXT("Modules used: application, system, display, buffer, graphic, ")
             MIL_TEXT("calibration, \n3d reconstruction.\n\n"));

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

///*****************************************************************************
// Structures, constants and functions
//*****************************************************************************
struct ROIStruct
   {
   MIL_INT OffsetX;
   MIL_INT OffsetY;
   MIL_INT SizeX;
   MIL_INT SizeY;
   };

// Source image files specification. 
static const MIL_INT STRING_LENGTH = 1024;

static const MIL_TEXT_CHAR DEPTH_MAP_IMAGE_FILE[STRING_LENGTH] = 
   M_IMAGE_PATH MIL_TEXT("3dPlaneFit/MechanicalPartDepthMap.mim");

static const MIL_TEXT_CHAR RECONSTRUCTION_IMAGE_FILE[STRING_LENGTH] = 
   M_IMAGE_PATH MIL_TEXT("3dPlaneFit/Inclination.mim");

static const MIL_TEXT_CHAR SIDE_VIEW_IMAGE_FILE[STRING_LENGTH] = 
   M_IMAGE_PATH MIL_TEXT("3dPlaneFit/SideView.png");

static const MIL_DOUBLE DISPLAY_ZOOM_FACTOR_X = 0.75;
static const MIL_DOUBLE DISPLAY_ZOOM_FACTOR_Y = 0.75;

static const ROIStruct PLANE_REGION =
   {
   388,
   349,
   75,
   75
   };

static const MIL_INT NUM_LOCATIONS = 8;

static const ROIStruct MEASURE_REGION[NUM_LOCATIONS] =
   {
      {400, 440, 20, 20},
      {316, 278, 20, 20},
      {572, 544, 20, 20},
      {660, 633, 20, 20},
      {132, 174, 20, 20},
      {580, 675, 20, 20},
      {626, 785, 10, 10},
      {600, 395, 20, 20}
   };

 //Functions
MIL_DOUBLE GetDistanceErrorFactor(MIL_ID MilGeometry);

//*****************************************************************************
// Main.
//*****************************************************************************
int MosMain(void)
   {
   MIL_ID            MilApplication,        // Application identifier.
                     MilDisplay,            // Display identifier.
                     MilSystem,             // System identifier.     
                     MilDepthMapImage,      // Image identifier for depth map
                     MilCalibration;        // Calibration identifier

   MIL_ID            MilIllustrationImage;  // Illustration image
   MIL_ID            MilIllustrationDisplay;// Illustration display

   MIL_ID            MilGeometry;           // 3dmap context for define a geometry
   MIL_ID            MilGraphics;           // Graphics context
   MIL_ID            MilGraphicsList;       // Graphics list

   // Allocate MIL objects. 
   MappAlloc(M_NULL, M_DEFAULT, &MilApplication);
   MsysAlloc(M_DEFAULT, M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, &MilSystem);
   MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, 
              &MilDisplay);
   MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, 
              &MilIllustrationDisplay);

   // Print Header. 
   PrintHeader();

   //Allocate the graphics context
   MgraAlloc(MilSystem, &MilGraphics);
   MgraAllocList(MilSystem, M_DEFAULT, &MilGraphicsList);

   //Set the color of the graphics
   MgraColor(MilGraphics, M_COLOR_GREEN);

   //Set the background mode
   MgraControl(MilGraphics, M_BACKGROUND_MODE, M_TRANSPARENT);

   //Associate the graphics list to the display
   MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraphicsList);

   //Restore the image with the depth map
   MbufRestore(DEPTH_MAP_IMAGE_FILE, MilSystem, &MilDepthMapImage);

   //Restore the calibration and associate it to the images
   McalRestore(DEPTH_MAP_IMAGE_FILE, MilSystem, M_DEFAULT, &MilCalibration);
   McalAssociate(MilCalibration, MilDepthMapImage, M_DEFAULT);

   //Display the depth map
   MdispZoom(MilDisplay, DISPLAY_ZOOM_FACTOR_X, DISPLAY_ZOOM_FACTOR_Y);
   MdispControl(MilDisplay, M_VIEW_MODE, M_AUTO_SCALE);
   MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_X, 0);
   MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_Y, 0);
   MdispControl(MilDisplay, M_TITLE, 
      (MIL_DOUBLE)M_PTR_TO_DOUBLE(MIL_TEXT("Depth Map")));
   MdispSelect(MilDisplay, MilDepthMapImage);

   //Restore the illustration image
   MbufRestore(RECONSTRUCTION_IMAGE_FILE, MilSystem, &MilIllustrationImage); 

   //Set display controls
   MIL_INT InitialWindowPositionX = 
      (MIL_INT)(MbufInquire(MilDepthMapImage, M_SIZE_X, M_NULL)*
                            DISPLAY_ZOOM_FACTOR_X+20);

   MdispControl(MilIllustrationDisplay, M_WINDOW_INITIAL_POSITION_X, 
      (MIL_DOUBLE)(InitialWindowPositionX)); 
   MdispControl(MilIllustrationDisplay, M_WINDOW_INITIAL_POSITION_Y, 0);
   MdispControl(MilIllustrationDisplay, M_TITLE, 
      (MIL_DOUBLE)M_PTR_TO_DOUBLE(MIL_TEXT("3D Reconstruction")));

   //Display the illustration image
   MdispSelect(MilIllustrationDisplay, MilIllustrationImage);   

   //Allocate a mask buffer for the region of the plane in the depth map
   MIL_ID MilMaskImage;

   MbufAlloc2d(MilSystem, MbufInquire(MilDepthMapImage, M_SIZE_X, M_NULL),
               MbufInquire(MilDepthMapImage, M_SIZE_Y, M_NULL),
               MbufInquire(MilDepthMapImage, M_TYPE, M_NULL),
               M_IMAGE+M_PROC, &MilMaskImage);

   //Clear the mask image
   MbufClear(MilMaskImage, 0);

   //Set the mask region to white
   MIL_ID MilMaskChild;

   MbufChild2d(MilMaskImage, PLANE_REGION.OffsetX, PLANE_REGION.OffsetY, 
      PLANE_REGION.SizeX, PLANE_REGION.SizeY, &MilMaskChild);
   MbufClear(MilMaskChild, 65535);

   //Show the region used to define the plane
   MgraRect(MilGraphics, MilGraphicsList, PLANE_REGION.OffsetX, PLANE_REGION.OffsetY,
      PLANE_REGION.OffsetX+PLANE_REGION.SizeX, PLANE_REGION.OffsetY+PLANE_REGION.SizeY);

   MosPrintf(MIL_TEXT("The displayed depth map of a 3d reconstruction shows a mechanical ")
             MIL_TEXT("part\nwith an inclination relative to the z plane as illustrated ")
             MIL_TEXT("on the right.\nUsing the depth map region shown in green, a plane ")
             MIL_TEXT("with this inclination\nwill be fitted and used to calculate ")
             MIL_TEXT("relative measurements for the object.\n"));
   MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));

   MosGetch();

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

   //Define the plane using the mask 
   M3dmapSetGeometry(MilGeometry, M_PLANE, M_FIT, (MIL_DOUBLE)MilDepthMapImage, 
      (MIL_DOUBLE)MilMaskImage, M_DEFAULT, M_DEFAULT, M_DEFAULT);

   //Get the multiplicative factor to correct error in the distance
   MIL_DOUBLE DistanceErrorFactor = GetDistanceErrorFactor(MilGeometry);

   if(M3dmapInquire(MilGeometry, M_DEFAULT, M_STATUS, M_NULL) == M_SUCCESS)
      {
      MdispControl(MilDisplay, M_UPDATE_GRAPHIC_LIST, M_DISABLE);

      //Change color for graphics
      MgraColor(MilGraphics, M_COLOR_CYAN);

      //Create a child for statistics calculations
      MIL_ID MilDepthMapChild;
      MIL_TEXT_CHAR OutputText[STRING_LENGTH] = MIL_TEXT("");
      MIL_DOUBLE AverageHeight=0.0;

      //Calculate and display the average difference in height
      for (MIL_INT i=0; i<NUM_LOCATIONS; i++)
         {
         MbufChild2d(MilDepthMapImage, MEASURE_REGION[i].OffsetX,
            MEASURE_REGION[i].OffsetY, MEASURE_REGION[i].SizeX, 
            MEASURE_REGION[i].SizeY, &MilDepthMapChild);
      
         //Get the average height
         M3dmapStat(MilDepthMapChild, MilGeometry, M_NULL, M_NULL, M_DEVIATION_MEAN, 
            M_DEFAULT, M_DEFAULT, &AverageHeight);

         //Correct the error
         AverageHeight *= DistanceErrorFactor;

         MgraRect(MilGraphics, MilGraphicsList, MEASURE_REGION[i].OffsetX, 
            MEASURE_REGION[i].OffsetY, MEASURE_REGION[i].OffsetX+MEASURE_REGION[i].SizeX, 
            MEASURE_REGION[i].OffsetY+MEASURE_REGION[i].SizeY);

         MosSprintf(OutputText, STRING_LENGTH, MIL_TEXT("%.2f"), AverageHeight);

         MgraText(MilGraphics, MilGraphicsList, 
            MEASURE_REGION[i].OffsetX+MEASURE_REGION[i].SizeX+10, 
            MEASURE_REGION[i].OffsetY, OutputText);

         MbufFree(MilDepthMapChild);
         }

      //Remove current illustration
      MdispSelect(MilIllustrationDisplay, M_NULL);

      MbufFree(MilIllustrationImage);

      //Show the side view illustration
      MbufRestore(SIDE_VIEW_IMAGE_FILE, MilSystem, &MilIllustrationImage);

      MdispControl(MilIllustrationDisplay, M_TITLE, 
         (MIL_DOUBLE)M_PTR_TO_DOUBLE(MIL_TEXT("Height side view")));

      MdispSelect(MilIllustrationDisplay, MilIllustrationImage);

      MdispControl(MilDisplay, M_UPDATE_GRAPHIC_LIST, M_ENABLE);

      MosPrintf(MIL_TEXT("The average differences in height of the shown regions are ")
                MIL_TEXT("displayed in\ncyan. These values are relative to the defined ")
                MIL_TEXT("plane, displayed in green.\n\n"));
      MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
      MosGetch();
      }
   else
      {
      MosPrintf(MIL_TEXT("Plane fit unsuccessful.\n"));
      MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
      MosGetch();
      }

   //Free all objects.
   MgraFree(MilGraphics);
   MgraFree(MilGraphicsList);
   M3dmapFree(MilGeometry);
   MbufFree(MilMaskChild);
   MbufFree(MilMaskImage);
   McalFree(MilCalibration);
   MbufFree(MilDepthMapImage);
   MbufFree(MilIllustrationImage);  
   MdispFree(MilDisplay);
   MdispFree(MilIllustrationDisplay);
   MsysFree(MilSystem);
   MappFree(MilApplication);
   return 0;
   }

//Calculate the error factor of the distances
MIL_DOUBLE GetDistanceErrorFactor(MIL_ID MilGeometry)
   {
   MIL_DOUBLE A, B, Z0;

   //Get the plane coefficients to determine the normal
   M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_AX, &A);
   M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_AY, &B);
   M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_Z0, &Z0);

   //Calculate the dot product between the normal of the plane
   //and the normal to the z=0 plane
   MIL_DOUBLE PlaneNormalsDotProduct = -Z0;

   //Get the length of the vectors
   MIL_DOUBLE RefVectorLength = sqrt(A*A+B*B+Z0*Z0);
   MIL_DOUBLE Z0VectorLength = 1;

   //Calculate the cos angle between the reference plane and the z=0
   //This gives the error factor on the distance.
   return PlaneNormalsDotProduct/(RefVectorLength*Z0VectorLength);      
   }