// +-------------------------------- DISCLAIMER ---------------------------------+
// |                                                                             |
// | This program is provided to you free of charge "as such".                   |
// | You should not assume that this program is error-free or suitable for any   |
// | purpose whatsoever.                                                         |
// |                                                                             |
// | EURESYS does not give any representation, warranty or undertaking that this |
// | program is free of any defect or error or suitable for any purpose. EURESYS |
// | shall not be liable, in contract, in torts or otherwise, for any damages,   |
// | loss, costs, expenses or other claims for compensation, including those     |
// | asserted by third parties, arising out of or in connection with the use of  |
// | this program.                                                               |
// +-----------------------------------------------------------------------------+

// Easy3DGrabDlg.cpp : implementation file
//

#include "../Common/stdafx.h"
#include "Easy3DGrabDlg.h"
#include "afxdialogex.h"
#include "Resource.h"
#include <algorithm>

// include the grabbers

#ifdef ATGRAB
#include "../ATGrabberApp/ATGrabber.h"
#elif AZUREKINECTGRAB
#include "../AzureKinectGrabberApp/AzureKinectGrabber.h"
#elif BENANOGRAB
#include "../BenanoGrabberApp/BenanoGrabber.h"
#elif DUMMYGRAB
#include "../DummyGrabberApp/DummyGrabber.h"
#elif IDSENSENSOGRAB
#include "../IDSEnsensoGrabberApp/IDSEnsensoGrabber.h"
#elif INTELRSGRAB
#include "../IntelRSGrabberApp/IntelRSGrabber.h"
#elif LMIGRAB
#include "../LMIGrabberApp/LMIGrabber.h"
#elif LUCIDHELIOSGRAB
#include "../LucidHeliosGrabberApp/LucidHeliosGrabber.h"
#elif MECHMINDGRAB
#include "../MechMindGrabberApp/MechMindGrabber.h"
#elif PHOTONEOGRAB
#include "../PhotoneoGrabberApp/PhotoneoGrabber.h"
#elif SSZNGRAB
#include "../SSZNGrabberApp/SSZNGrabber.h"
#elif ZIVIDGRAB
#include "../ZividGrabberApp/ZividGrabber.h"
#elif NERIANGRAB
#include "../NerianGrabberApp/NerianGrabber.h"
#elif HELIOTISGRAB
#include "../HeliotisGrabberApp/HeliotisGrabber.h"
#endif


#ifdef _DEBUG
#define new DEBUG_NEW
#endif


// CAboutDlg dialog used for App About

class CAboutDlg : public CDialogEx
{
public:
  CAboutDlg();

  // Dialog Data
#ifdef AFX_DESIGN_TIME
  enum { IDD = IDD_ABOUTBOX };
#endif

protected:
  virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support

// Implementation
protected:
  DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialogEx(IDD_ABOUTBOX)
{
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
  CDialogEx::DoDataExchange(pDX);
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialogEx)
END_MESSAGE_MAP()


// Easy3DGrabDlg dialog



Easy3DGrabDlg::Easy3DGrabDlg(CWnd* pParent /*=nullptr*/)
  : CDialogEx(IDD_EASY3DGRAB_DIALOG, pParent), depth_map(NULL), zmap(NULL), ground_leveled(false)
{
  m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

Easy3DGrabDlg::~Easy3DGrabDlg()
{}

void Easy3DGrabDlg::DoDataExchange(CDataExchange* pDX)
{
  CDialogEx::DoDataExchange(pDX);
  DDX_Control(pDX, IDC_DEPTHMAP_VIEW, depthmap_view);
  DDX_Control(pDX, IDC_POINTCLOUD_VIEW, pointcloud_view);
  DDX_Control(pDX, IDC_ZMAP_VIEW, zmap_view);
  DDX_Control(pDX, IDC_COMBO_OBJECT_INFO, m_object_info_ctrl);
}

BEGIN_MESSAGE_MAP(Easy3DGrabDlg, CDialogEx)
  ON_WM_SYSCOMMAND()
  ON_WM_PAINT()
  ON_WM_QUERYDRAGICON()
  ON_BN_CLICKED(IDC_GRAB_BUTTON, &Easy3DGrabDlg::OnBnClickedGrab)
  ON_BN_CLICKED(IDC_PROPERTIES_BUTTON, &Easy3DGrabDlg::OnClickedPropertiesButton)
  ON_BN_CLICKED(IDC_SAVE_DEPTHMAP, &Easy3DGrabDlg::OnClickedSaveDepthmap)
  ON_BN_CLICKED(IDC_SAVE_POINTCLOUD, &Easy3DGrabDlg::OnClickedSavePointCloud)
  ON_BN_CLICKED(IDC_SAVE_ZMAP, &Easy3DGrabDlg::OnClickedSaveZMap)
  ON_BN_CLICKED(IDC_EXTRACT, &Easy3DGrabDlg::OnClickedExtract)
  ON_BN_CLICKED(IDC_GROUND_LEVEL, &Easy3DGrabDlg::OnClickedGroundLevel)
  ON_CBN_SELCHANGE(IDC_COMBO_OBJECT_INFO, &Easy3DGrabDlg::OnClickedObjectInfo)
END_MESSAGE_MAP()


// Easy3DGrabDlg message handlers

BOOL Easy3DGrabDlg::OnInitDialog()
{
  CDialogEx::OnInitDialog();

  // Add "About..." menu item to system menu.

  // IDM_ABOUTBOX must be in the system command range.
  ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
  ASSERT(IDM_ABOUTBOX < 0xF000);

  CMenu* pSysMenu = GetSystemMenu(FALSE);
  if (pSysMenu != nullptr)
  {
    BOOL bNameValid;
    CString strAboutMenu;
    bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);
    ASSERT(bNameValid);
    if (!strAboutMenu.IsEmpty())
    {
      pSysMenu->AppendMenu(MF_SEPARATOR);
      pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
    }
  }

  // Set the icon for this dialog
  SetIcon(m_hIcon, TRUE);			// Set big icon

  // check Easy3DObject license
  if (!Easy::CheckLicense(LicenseFeatures::Features::Easy3DObject))
    GetDlgItem(IDC_EXTRACT)->EnableWindow(FALSE);

  // Init frame grabber
  try
  {
#ifdef ATGRAB
    grabber = new ATGrabber("169.254.64.2");
#elif AZUREKINECTGRAB
    grabber = new AzureKinectGrabber();
#elif BENANOGRAB
    grabber = new BenanoGrabber();
#elif DUMMYGRAB
    grabber = new DummyGrabber();
#elif IDSENSENSOGRAB
    grabber = new IDSEnsensoGrabber();
#elif INTELRSGRAB
    grabber = new IntelRSGrabber();
#elif LMIGRAB
    grabber = new LMIGrabber("192.168.1.10");
#elif LUCIDHELIOSGRAB
    grabber = new LucidHeliosGrabber();
#elif MECHMINDGRAB
    grabber = new MechMindGrabber("192.168.1.118");
#elif PHOTONEOGRAB
    grabber = new PhotoneoGrabber();
#elif SSZNGRAB
    grabber = new SSZNGrabber("192.168.0.126");
#elif ZIVIDGRAB
    grabber = new ZividGrabber();
#elif NERIANGRAB
    grabber = new NerianGrabber();
#elif HELIOTISGRAB
    grabber = new HeliotisGrabber();
#endif
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Grab"), MB_OK | MB_ICONSTOP);
  }

  // init object info
  m_object_info_ctrl.SetWindowTextW(L"None");

  return TRUE;  // return TRUE unless you set the focus to a control
}

void Easy3DGrabDlg::OnCancel()
{
  delete grabber;
  delete depth_map;
  delete zmap;

  CDialogEx::OnCancel();
}

void Easy3DGrabDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
  if ((nID & 0xFFF0) == IDM_ABOUTBOX)
  {
    CAboutDlg dlgAbout;
    dlgAbout.DoModal();
  }
  else
  {
    CDialogEx::OnSysCommand(nID, lParam);
  }
}

// If you add a minimize button to your dialog, you will need the code below
// to draw the icon.  For MFC applications using the document/view model,
// this is automatically done for you by the framework.
void Easy3DGrabDlg::OnPaint()
{
  if (IsIconic())
  {
    CPaintDC dc(this); // device context for painting

    SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

    // Center icon in client rectangle
    int cxIcon = GetSystemMetrics(SM_CXICON);
    int cyIcon = GetSystemMetrics(SM_CYICON);
    CRect rect;
    GetClientRect(&rect);
    int x = (rect.Width() - cxIcon + 1) / 2;
    int y = (rect.Height() - cyIcon + 1) / 2;

    // Draw the icon
    dc.DrawIcon(x, y, m_hIcon);
  }
  else
  {
    CDialogEx::OnPaint();
  }
}

// The system calls this function to obtain the cursor to display while the user drags
// the minimized window.
HCURSOR Easy3DGrabDlg::OnQueryDragIcon()
{
  return static_cast<HCURSOR>(m_hIcon);
}

void Easy3DGrabDlg::OnBnClickedGrab()
{
  // Grab next frame
  try
  {
    grabber->Start();
    long long int frame_id = grabber->WaitFrame(20000000);
    if (frame_id)
    {
      try
      {
        if (depth_map == NULL)
        {
          switch (grabber->GetBitDepth())
          {
          case 8:
            depth_map = new Easy3D::EDepthMap8();
            break;
          case 16:
            depth_map = new Easy3D::EDepthMap16();
            break;
          case 32:
            depth_map = new Easy3D::EDepthMap32f();
            break;
          }
        }
        grabber->GetDepthMap(*depth_map);
        depthmap_view.SetDepthMap(depth_map);
      }
      catch (...)
      {
      }

      try
      {
        grabber->GetPointCloud(point_cloud);
        pointcloud_view.SetPointCloud(point_cloud);
      }
      catch (...)
      {
      }

      try
      {
        if (zmap == NULL)
        {
          switch (grabber->GetBitDepth())
          {
          case 8:
            zmap = new Easy3D::EZMap8();
            break;
          case 16:
            zmap = new Easy3D::EZMap16();
            break;
          case 32:
            zmap = new Easy3D::EZMap32f();
            break;
          }
        }

        grabber->GetZMap(*zmap);
        zmap_view.SetZMap(zmap);
      }
      catch (...)
      {
      }

      grabber->ReleaseFrame();

      int width = depth_map->GetWidth();
      int height = depth_map->GetHeight();
      if (width == 0 || height == 0)
      {
        width = zmap->GetWidth();
        height = zmap->GetHeight();
      }

      char message[256];
      sprintf_s(message, sizeof(message), "Frame %lld: %dx%d", frame_id, width, height);
      SetDlgItemText(IDC_STATUS_TEXT, CString(message));
    }
    grabber->Stop();
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Grab"), MB_OK | MB_ICONSTOP);
  }
}

void Easy3DGrabDlg::OnClickedPropertiesButton()
{
  try
  {
    grabber->InvokeParameterPanel();
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Grab"), MB_OK | MB_ICONSTOP);
  }
}

void Easy3DGrabDlg::OnClickedSaveDepthmap()
{
  if (depth_map != nullptr && !depth_map->IsVoid())
  {
    CFileDialog FileDlg(FALSE, NULL, NULL, OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT, NULL, NULL, 0, TRUE);

    if (FileDlg.DoModal() == IDOK)
    {
      // Extract the path string
      std::string str = CT2A(FileDlg.GetPathName());
      try
      {
        depth_map->SaveImage(str);
      }
      catch (EException exc)
      {
        ::MessageBox(NULL, CString(exc.what()), _T("Save Depth Map"), MB_OK | MB_DEFAULT_DESKTOP_ONLY);
      }
    }
  }
  else
  {
    ::MessageBox(NULL, _T("Please grab data before saving Depth Map."), _T("Error saving Depth Map"), MB_OK | MB_ICONSTOP);
  }
}

void Easy3DGrabDlg::OnClickedSavePointCloud()
{
  if (point_cloud.GetNumPoints() > 0)
  {
    TCHAR szFilters[] = _T("PCD (*.pcd)|*.pcd|")
      _T("CSV (*.csv)|*.csv|")
      _T("OBJ (*.obj)|*.obj|")
      _T("PLY (*.ply)|*.ply|")
      _T("XYZ (*.xyz)|*.xyz|")
      _T("Open eVision|*.*|");

    CFileDialog FileDlg(FALSE, _T("pcd"), NULL, OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT, szFilters, NULL, 0, TRUE);

    if (FileDlg.DoModal() == IDOK)
    {
      // Extract the path string
      std::string path = CT2A(FileDlg.GetPathName());
      try
      {
        point_cloud.Save(path);
      }
      catch (EException exc)
      {
        ::MessageBox(NULL, CString(exc.what()), _T("Save Point Cloud"), MB_OK | MB_DEFAULT_DESKTOP_ONLY);
      }
    }
  }
  else
  {
    ::MessageBox(NULL, _T("Please grab data before saving Point Cloud."), _T("Error saving Point Cloud"), MB_OK | MB_ICONSTOP);
  }
}

void Easy3DGrabDlg::OnClickedSaveZMap()
{
  if (zmap != nullptr && !zmap->IsVoid())
  {
    CFileDialog FileDlg(FALSE, NULL, NULL, OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT, NULL, NULL, 0, TRUE);

    if (FileDlg.DoModal() == IDOK)
    {
      // Extract the path string
      std::string str = CT2A(FileDlg.GetPathName());
      try
      {
        zmap->SaveImage(str);
      }
      catch (EException exc)
      {
        ::MessageBox(NULL, CString(exc.what()), _T("Save ZMap"), MB_OK | MB_DEFAULT_DESKTOP_ONLY);
      }
    }
  }
  else
  {
    ::MessageBox(NULL, _T("Please grab data before saving ZMap."), _T("Error saving ZMap"), MB_OK | MB_ICONSTOP);
  }
}

void Easy3DGrabDlg::OnClickedExtract()
{
  if (!zmap)
  {
    ::MessageBox(NULL, _T("Please grab data before extracting objects."), _T("Error extracting objects"), MB_OK | MB_ICONSTOP);
    return;
  }

  try
  {
    // Perform Easy3DObject extraction (if available) on ZMap
    if (!zmap->IsVoid())
    {
      // Find ZMap x,y,z world extent
      EPoint worldSize = zmap->GetSizeInWorld();
      float zmap_min = std::min(worldSize.GetX(), worldSize.GetY()) / 25.f;
      float zmap_max = std::max(worldSize.GetX(), worldSize.GetY()) / 2.f;

      float zrange;
      if (zmap->GetType() == EImageType_Depth32f)
      {
        float min_pix = FLT_MAX, max_pix = -FLT_MAX;
        int width = zmap->GetWidth();
        int height = zmap->GetHeight();
        for (int y = 0; y < height; ++y)
        {
          const auto src = (float*)zmap->GetBufferPtr(0, y);
          for (int x = 0; x < width; ++x)
          {
            if (src[x] < min_pix)
              min_pix = src[x];

            if (src[x] > max_pix)
              max_pix = src[x];
          }
        }
        zrange = (max_pix - min_pix) * zmap->GetZResolution();
      }
      else if (zmap->GetType() == EImageType_Depth16)
      {
        EImageStatisticsBW16 stats;
        stats.SetImage(&dynamic_cast<Easy3D::EZMap16*>(zmap)->AsEImage());

        EBW16 min_pix = stats.GetPixelMin();
        EBW16 max_pix = stats.GetPixelMax();

        zrange = (max_pix.Value - min_pix.Value) * zmap->GetZResolution();
      }
      else // EImageType_Depth8
      {
        EImageStatisticsBW8 stats;
        stats.SetImage(&dynamic_cast<Easy3D::EZMap8*>(zmap)->AsEImage());

        EBW8 min_pix = stats.GetPixelMin();
        EBW8 max_pix = stats.GetPixelMax();

        zrange = (max_pix.Value - min_pix.Value) * zmap->GetZResolution();
      }

      // Setup a 3D object extractor with reasonable values
      Easy3D::E3DObjectExtractor extractor;

      extractor.SetLengthRange(EFloatRange(zmap_min, zmap_max));
      extractor.SetWidthRange(EFloatRange(zmap_min, zmap_max));
      extractor.SetLocalHeightRange(EFloatRange(zrange / 100.f, 1.5f*zrange));
      extractor.SetReferenceHeightRange(EFloatRange(zrange / 100.f, 1.5f*zrange));

      extractor.SetLocalTiltRange(EFloatRange(0.f, 90.f));
      extractor.SetReferenceTiltRange(EFloatRange(0.f, 90.f));
      extractor.SetOrientationRange(EFloatRange(-90.f, 90.f));
      extractor.SetAspectRatioRange(EFloatRange(0.1f, 0.9f));

      extractor.SetAreaRange(EFloatRange(zmap_max*zmap_max / 100.f, zmap_max*zmap_max));
      extractor.SetVolumeRange(EFloatRange(zmap_max*zmap_max*zrange / 1000.f, zmap_max*zmap_max*zrange));

      // Perform the extraction
      extractor.Extract(zmap);
      std::vector<Easy3D::E3DObject> obj_list = extractor.GetObjects();

      // Display objects as ZMap overlay
      zmap_view.SetObjectList(obj_list);

      // Transform the 3D objects from ZMap space to World space
      const Easy3D::E3DTransformMatrix map2world = zmap->GetMapToWorldMatrix();
      std::vector<Easy3D::E3DObject> world_obj_list;
      for (auto& obj : obj_list)
      {
        Easy3D::E3DObject world_obj = obj.Transform(map2world);
        world_obj_list.push_back(world_obj);
      }

      // Display objects as PointCloud overlay
      pointcloud_view.SetObjectList(world_obj_list);

      // Update status message
      char message[256];
      sprintf_s(message, sizeof(message), "%d objects extracted", (int)obj_list.size());
      SetDlgItemText(IDC_STATUS_TEXT, CString(message));
    }
  }
  catch (EException e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Object extraction"), MB_OK | MB_ICONSTOP);
  }
}

void Easy3DGrabDlg::OnClickedGroundLevel()
{
  if (!zmap)
  {
    ::MessageBox(NULL, _T("Please grab data before ground leveling."), _T("Error ground leveling"), MB_OK | MB_ICONSTOP);
    return;
  }

  ground_leveled = false;

  // estimate a deviation for the plane finder, based on point cloud size
  EPoint worldSize = zmap->GetSizeInWorld();
  float plane_range = 0.001f * (worldSize.GetX() + worldSize.GetY());

  // search for the ground plane
  Easy3D::EPlaneFinder plane_finder(plane_range);
  float ratio = 0.f;
  bool base_plane_found = false;
  try
  {
    ratio = plane_finder.Find(point_cloud, ground_plane);
    base_plane_found = true;
  }
  catch (EException& e)
  {
    ::MessageBoxA(NULL, e.what(), "Ground plane not found.", MB_OK | MB_ICONSTOP);
  }

  if (base_plane_found)
  {
    // shift down the ground plane
    ground_plane.SetSignedDistanceFromOrigin(ground_plane.GetSignedDistanceFromOrigin() - 5 * plane_range);

    // Build the new ZMap using the ground plane as reference
    Easy3D::EPointCloudToZMapConverter ptoz_converter;
    ptoz_converter.SetReferencePlane(ground_plane);
    ptoz_converter.Convert(point_cloud, zmap);

    zmap->ResetWorldTransformation();
    zmap_view.SetZMap(zmap);

    // rebuild a point cloud in the ZMap space
    point_cloud.Clear();
    Easy3D::EZMapToPointCloudConverter ztop_converter;
    ztop_converter.Convert(zmap, point_cloud, true);

    pointcloud_view.SetPointCloud(point_cloud);

    ground_leveled = true;
  }
}

void Easy3DGrabDlg::OnClickedObjectInfo()
{
  int sel = m_object_info_ctrl.GetCurSel();
  pointcloud_view.UpdateObjectInformation(sel);
}

