/************************************************************************************/
/*
* File name: RangeToPointCloudConverter.cpp
* Location: See Matrox Example Launcher in the MIL Control Center
 * 
*
* Synopsis:  This file contains the implementation of the CRangeToPointCloudConverter class that
*            converts the range image to a point cloud usable by MIL.
*
* Copyright (C) Matrox Electronic Systems Ltd., 1992-2016.
* All Rights Reserved
*/

#include <mil.h>
#include <vector>
using std::vector;
#include "RangeToPointCloudConverter.h"

//*****************************************************************************
// Constructor.
//*****************************************************************************
CRangeToPointCloudConverter::CRangeToPointCloudConverter(MIL_ID MilCal,
                                                         MIL_INT RangeSizeX,
                                                         MIL_INT RangeSizeY) :
   m_RangeSizeX(RangeSizeX),
   m_RangeSizeY(RangeSizeY)
   {
   MIL_INT TotalNbPixels = RangeSizeX * RangeSizeY;
   vector<MIL_DOUBLE> XPixel(TotalNbPixels);
   vector<MIL_DOUBLE> YPixel(TotalNbPixels);
   m_XUnit.resize(TotalNbPixels);
   m_YUnit.resize(TotalNbPixels);
   m_ZUnit.resize(TotalNbPixels);
   m_XWorld.resize(TotalNbPixels);
   m_YWorld.resize(TotalNbPixels);
   m_ZWorld.resize(TotalNbPixels);

   // Fill the pixel coordinates.
   for(MIL_INT Y = 0, i = 0; Y < RangeSizeY; Y++)
      {
      for(MIL_INT X = 0; X < RangeSizeX; X++, i++)
         {
         XPixel[i] = (MIL_DOUBLE)X;
         YPixel[i] = (MIL_DOUBLE)Y;
         }
      }

   // Get the unit vectors of each pixel of the range image.
   McalTransformCoordinate3dList(MilCal,
                                 M_PIXEL_COORDINATE_SYSTEM,
                                 M_CAMERA_COORDINATE_SYSTEM,
                                 TotalNbPixels,
                                 &XPixel[0],
                                 &YPixel[0],
                                 M_NULL,
                                 &m_XUnit[0],
                                 &m_YUnit[0],
                                 &m_ZUnit[0],
                                 M_UNIT_DIRECTION_VECTOR);
   }

//*****************************************************************************
// Destructor.
//*****************************************************************************
CRangeToPointCloudConverter::~CRangeToPointCloudConverter()
   {
   }

//*****************************************************************************
// CreatePointCloud. Creates the point cloud from a range image based on the
//                   unit vector of the camera calibration. The method
//                   can accept some offset and scale between the range gray value
//                   and the calibrated world units.
//*****************************************************************************
void CRangeToPointCloudConverter::CreatePointCloud(MIL_ID MilRangeImage, MIL_ID MilPointCloud, MIL_ID MilCal,
                                                   MIL_DOUBLE RangeOffset, MIL_DOUBLE RangeScale)
   {
   MIL_UINT16* pRangeData = (MIL_UINT16*)MbufInquire(MilRangeImage, M_HOST_ADDRESS, M_NULL);
   MIL_INT RangePitch = MbufInquire(MilRangeImage, M_PITCH, M_NULL);
   MIL_INT UnitIdx = 0;
   for(MIL_INT Y = 0; Y < m_RangeSizeY; Y++)
      {
      for(MIL_INT X = 0; X < m_RangeSizeX; X++)
         {
         if(pRangeData[X] != 0)
            {
            // Calculate the world position of the point in the depth image.
            MIL_DOUBLE RangeWorld = pRangeData[X] * RangeScale + RangeOffset;
            m_XWorld[UnitIdx] = RangeWorld * m_XUnit[UnitIdx];
            m_YWorld[UnitIdx] = RangeWorld * m_YUnit[UnitIdx];
            m_ZWorld[UnitIdx] = RangeWorld * m_ZUnit[UnitIdx];
            }
         else
            {
            m_XWorld[UnitIdx] = M_INVALID_POINT;
            m_YWorld[UnitIdx] = M_INVALID_POINT;
            m_ZWorld[UnitIdx] = M_INVALID_POINT;
            }
         UnitIdx++;
         }
      pRangeData += RangePitch;
      }

   // Put the relative coordinate system on the camera coordinate system.
   McalSetCoordinateSystem(MilCal,
                           M_RELATIVE_COORDINATE_SYSTEM,
                           M_CAMERA_COORDINATE_SYSTEM,
                           M_IDENTITY,
                           M_NULL,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT);

   // Add the points to the point cloud.
   M3dmapPut(MilPointCloud, M_POINT_CLOUD_LABEL(1), M_POSITION, 64 + M_FLOAT,
             m_XWorld.size(), &m_XWorld[0], &m_YWorld[0], &m_ZWorld[0], MilCal, M_DEFAULT);

   // Put the relative coordinate system back on the absolute coordinate system.
   McalSetCoordinateSystem(MilPointCloud,
                           M_RELATIVE_COORDINATE_SYSTEM,
                           M_ABSOLUTE_COORDINATE_SYSTEM,
                           M_IDENTITY,
                           M_NULL,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT);
   }