//*******************************************************************************
// 
// File name: D3DSysDisplayManager.cpp
// Location: See Matrox Example Launcher in the MIL Control Center
// 
//
// Synopsis:  Class in charge of managing the D3D Sys displays for 3D analysis 
//            examples.
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2016.
// All Rights Reserved

#include "BaseCommon.h"

MIL_UINT32 MFTYPE UpdateThreadStatic(void* pUserDataPtr) 
   {
   CD3DSysDisplayManager* pThis = static_cast<CD3DSysDisplayManager*>(pUserDataPtr);
   return pThis->UpdateThread();
   }

//*******************************************************************************
// Initializes the object.
//*******************************************************************************
CD3DSysDisplayManager::CD3DSysDisplayManager() :
   m_NumCamLaserPairs(0),
   m_MilSystem(0),
   m_ReadyForRefreshEvent(M_NULL),
   m_EndEvent(M_NULL),
   m_UpdateThread(M_NULL),
   m_Mutex(M_NULL),
   m_pCameraLaserCtxs(NULL),
   m_LimitationWarningVerified(false)
   {
   }

//*******************************************************************************
// Allocates the 3d display objects and starts the update thread.
//*******************************************************************************
bool CD3DSysDisplayManager::Alloc(MIL_ID MilSystem,
                                  MIL_ID* pCameraLaserCtxs,
                                  MIL_INT NumCamLaserPairs,
                                  const SMapGeneration* MapVisualizationData)
   {
   bool Success = false;

#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT

   Free();

   m_NumCamLaserPairs = NumCamLaserPairs;
   m_MilSystem = MilSystem;

   m_DispHandle = MdepthSysD3DAlloc(pCameraLaserCtxs,
                                    M_NULL,
                                    M_NULL,
                                    M_NULL,
                                    M_NULL,
                                    M_NULL,
                                    m_NumCamLaserPairs, 
                                    D3D_DISPLAY_SIZE_X, 
                                    D3D_DISPLAY_SIZE_Y, 
                                    MAX_DISTANCE_Z,
                                    M_NULL);
   Success = (m_DispHandle != M_NULL);
   m_LimitationWarningVerified = false;

   if (Success)
      {
      MthrAlloc(MilSystem, M_EVENT, M_NOT_SIGNALED + M_AUTO_RESET  , M_NULL, M_NULL, &m_ReadyForRefreshEvent);
      MthrAlloc(MilSystem, M_EVENT, M_NOT_SIGNALED + M_MANUAL_RESET, M_NULL, M_NULL, &m_EndEvent);

      MthrAlloc(MilSystem, M_MUTEX, M_DEFAULT , M_NULL, M_NULL, &m_Mutex);
      MthrAlloc(MilSystem, M_THREAD, M_DEFAULT, UpdateThreadStatic, this, &m_UpdateThread);
      }

#endif

   return Success;
   }

//*******************************************************************************
// Frees the display.
//*******************************************************************************
void CD3DSysDisplayManager::Free()
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   Hide();
   if (m_DispHandle)
      {
      // End the update thread
      MthrControl(m_EndEvent, M_EVENT_SET, M_SIGNALED);
      MthrWait(m_UpdateThread, M_THREAD_END_WAIT, M_NULL);
      MthrFree(m_UpdateThread);
      MthrFree(m_EndEvent);
      MthrFree(m_ReadyForRefreshEvent);

      MthrFree(m_Mutex);

      m_pCameraLaserCtxs = NULL;

      MdispD3DFree(m_DispHandle);
      m_DispHandle = M_NULL;

      m_LimitationWarningVerified = false;
      }
#endif
   }

//*******************************************************************************
// Shows the display.
//*******************************************************************************
void CD3DSysDisplayManager::Show()
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   if (m_DispHandle != M_NULL && !m_Showing)
      {
      MdispD3DShow(m_DispHandle);
      m_Showing = true;
      }
#endif
   }

//*******************************************************************************
// Hides the display.
//*******************************************************************************
void CD3DSysDisplayManager::Hide()
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   if (m_Showing)
      {
      MdispD3DHide(m_DispHandle);
      m_Showing = false;
      }
#endif
   }

//*******************************************************************************
// Refresh the display with latest laser scan for given index.
//*******************************************************************************
void CD3DSysDisplayManager::Refresh3d(MIL_ID*  pCameraLaserCtxs,
                                      MIL_ID*  pLineImages,
                                      MIL_ID*  pPtCldCtr,
                                      MIL_INT* pPtCldLabels,
                                      MIL_ID*  pDepthmaps)
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   if(!m_Showing)
      { Show(); }

   // This object is not modified, no need to make a copy for the display.
   m_pCameraLaserCtxs = pCameraLaserCtxs;

   Lock();
   CopyObjects(pLineImages,
               pPtCldCtr,
               pPtCldLabels,
               pDepthmaps);
   Unlock();
   
   // Trigger the display refresh thread.
   MthrControl(m_ReadyForRefreshEvent, M_EVENT_SET, M_SIGNALED);
#endif
   }

//*******************************************************************************
// Print the help for the 3d display.
//*******************************************************************************
void CD3DSysDisplayManager::PrintHelp() const
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   if(m_DispHandle != M_NULL)
      { MdispD3DPrintHelp(m_DispHandle); }
#endif
   }

//*******************************************************************************
// Lock the mutex.
//*******************************************************************************
void CD3DSysDisplayManager::Lock()
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   MthrControl(m_Mutex, M_LOCK, M_DEFAULT);
#endif
   }

//*******************************************************************************
// Unlock the mutex.
//*******************************************************************************
void CD3DSysDisplayManager::Unlock()
   {   
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   MthrControl(m_Mutex, M_UNLOCK, M_DEFAULT);
#endif
   }

//*******************************************************************************
// Wait on event to update the display.
//*******************************************************************************
MIL_UINT32 CD3DSysDisplayManager::UpdateThread()
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   MIL_ID EventsToWaitOn[MAX_NB_CAMERAS + 1];
   EventsToWaitOn[0] = m_ReadyForRefreshEvent;
   EventsToWaitOn[1] = m_EndEvent;

   bool UpdateLoop = true;
   while(UpdateLoop)
      {
      MIL_INT WaitState = MthrWaitMultiple(&EventsToWaitOn[0], 2, M_EVENT_WAIT, M_NULL);
      if((0 == WaitState)) // ReadyForRefreshEvent
         {
         UpdateDisplay();
         }
      else
         {
         // Any other state will stop the update loop
         UpdateLoop = false;
         }
      }

#endif
   return 0;
   }

//*******************************************************************************
// Update the 3d display.
//*******************************************************************************
void CD3DSysDisplayManager::UpdateDisplay()
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   if (m_Showing && m_DisplayUpdateEnabled)
      {
      // Will use the previously copied
      // objects to refresh the 3d display.
      MdepthSysD3DSetSystems(m_DispHandle, 
                             m_pCameraLaserCtxs,
                             M_NULL,
                             M_NULL, 
                             M_NULL,
                             M_NULL,
                             M_NULL,
                             m_NumCamLaserPairs, 
                             MAX_DISTANCE_Z);

      if(!m_LimitationWarningVerified)
         {
         // On the first update, verify if there's 
         // any 3d display limitation depending on the display device.
         MIL_DOUBLE SubX, SubY;
         MdispD3DInquire(m_DispHandle, M_DEFAULT, MD3D_SUBSAMPLE_X, &SubX);
         MdispD3DInquire(m_DispHandle, M_DEFAULT, MD3D_SUBSAMPLE_Y, &SubY);

         if((SubX != 1.0) || (SubY != 1.0))
            {
            MosPrintf(MIL_TEXT("\n"));
            MosPrintf(MIL_TEXT("+--------------------+\n"));
            MosPrintf(MIL_TEXT("+ 3d display warning +\n"));
            MosPrintf(MIL_TEXT("+-+------------------+\n"));
            MosPrintf(MIL_TEXT("  | The displayed point cloud has been subsampled\n"));
            MosPrintf(MIL_TEXT("  | due to limitations of the display device.\n"));
            MosPrintf(MIL_TEXT("  | Acquisition resolution is higher than it appears in 3d.\n"));
            MosPrintf(MIL_TEXT("  | Used subsampling factors are X: %.2f and Y: %.2f\n"), SubX, SubY);
            MosPrintf(MIL_TEXT("\n"));
            }

         m_LimitationWarningVerified = true;
         }
      }
#endif
   }

//*******************************************************************************
// Copies the objects to display in 3d.
//*******************************************************************************
void CD3DSysDisplayManager::CopyObjects(MIL_ID*  pLineImages,
                                        MIL_ID*  pPtCldCtr,
                                        MIL_INT* pPtCldLabels,
                                        MIL_ID*  pDepthmaps)
   {
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT
   MdepthSysD3DCopySystems(m_DispHandle,
                           m_pCameraLaserCtxs,
                           pLineImages,
                           pPtCldCtr, 
                           pPtCldLabels,
                           pDepthmaps,
                           M_NULL,
                           m_NumCamLaserPairs);
#endif
   }