// +-------------------------------- 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.                                                               |
// +-----------------------------------------------------------------------------+

#include "../Common/stdafx.h"
#include "../Common/Resource.h"
#include "MechMindGrabber.h"
#include "MechMindDialog.h"
#include <vector>
#include <sstream>
#include <limits>

#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/persistence.hpp>


MechMindGrabber::MechMindGrabber(const std::string& device_name) : started(false), locked(false), frame_counter(0)
{
  try
  {
    // Connecting to camera
    if (!camera.connect(device_name))
    {
      // Connection to camera fails
      throw(std::runtime_error("Connection to the camera failed"));
    }
    /*
    std::cout
      << "Camera ID: " << camera.getCameraId() << std::endl
      << "Version: " << camera.getCameraVersion() << std::endl
      << "Color Image Size: " << camera.getColorImgSize() << std::endl
      << "Depth Image Size: " << camera.getDepthImgSize() << std::endl; //get and print some information about camera device
    */
  }
  catch (const std::exception& e)
  {
    throw(std::runtime_error(e.what()));
  }
}

MechMindGrabber::~MechMindGrabber()
{}

void MechMindGrabber::Start(int num_frame)
{
  if (started)
    throw(std::runtime_error("MechMindGrabber::Start: grabber already started"));

  const pcl::PointCloud<pcl::PointXYZRGB> rgbCloud = camera.captureRgbPointCloud();
  ConvertPointCloud(rgbCloud);

  started = true;
}

long long int MechMindGrabber::WaitFrame(int timeout_ms)
{
  if (!started)
    throw(std::runtime_error("MechMindGrabber::WaitFrame: grabber not started"));

  locked = true;

  // Nothing to do

  ++frame_counter;
  return frame_counter;
}

void MechMindGrabber::ReleaseFrame()
{
  if (!locked)
    throw(std::runtime_error("MechMindGrabber::ReleaseFrame: frame not locked"));

  // Nothing to do

  locked = false;
}

void MechMindGrabber::Stop()
{
  if (!started)
    throw(std::runtime_error("MechMindGrabber::Stop: grabber not started"));

  // Nothing to do

  started = false;
}

int MechMindGrabber::GetBitDepth()
{
  return BIT_DEPTH;
}

void MechMindGrabber::GetDepthMap(Easy3D::EDepthMap& map)
{
  if (!locked)
    throw(std::runtime_error("Frame not available"));

  if (map.GetType() != EImageType_Depth8)
    throw(std::runtime_error("Incompatible Depth Map format"));

  // No depthmap available
}

void MechMindGrabber::GetPointCloud(Easy3D::EPointCloud& dst_pc)
{
  if (!locked)
    throw(std::runtime_error("Frame not available"));

  dst_pc = pointcloud;
}

void MechMindGrabber::GetZMap(Easy3D::EZMap& dst_zmap)
{
  if (!locked)
    throw(std::runtime_error("Frame not available"));

  if (dst_zmap.GetType() != EImageType_Depth8)
    throw(std::runtime_error("Incompatible Depth Map format"));

  Easy3D::EPointCloudToZMapConverter converter;
  converter.Convert(pointcloud, &dst_zmap);
}

void MechMindGrabber::InvokeParameterPanel()
{
  std::string error;

  std::string exposureMode = camera.getParameter("scan2dExposureMode", error);
  if (error != "")
    throw(std::runtime_error(error));

  std::string exposureTime = camera.getParameter("scan2dExposureTime", error);
  if (error != "")
    throw(std::runtime_error(error));

  MechMindParamDialog param_dialog(exposureMode, exposureTime);

  if (param_dialog.DoModal() == IDOK)
  {
    std::string status = camera.setParameter("scan2dExposureMode", std::stod(param_dialog.exposureMode));
    if (status != "Set scan2dExposureMode successfully!")
      throw(std::runtime_error(status));

    status = camera.setParameter("scan2dExposureTime", std::stod(param_dialog.exposureTime));
    if (status != "Set scan2dExposureTime successfully!")
      throw(std::runtime_error(status));
  }
}

void MechMindGrabber::ConvertPointCloud(const pcl::PointCloud<pcl::PointXYZRGB>& rgbCloud)
{
  const size_t nbPoints = rgbCloud.size();

  std::vector<Easy3D::E3DPoint> points;
  points.reserve(nbPoints);
  std::vector<EC24A> colors;

  for (size_t i = 0; i < nbPoints; ++i)
  {
    const pcl::PointXYZRGB& p = rgbCloud[i];
    if (p.x != 0 && p.y != 0 && p.z != 0)
    {
      points.emplace_back(-p.x, p.y, -p.z);
      float color = p.rgb;
      colors.emplace_back(p.r, p.g, p.b, 255);
    }
  }

  pointcloud.Clear();
  pointcloud.AddPoints(points);
  pointcloud.FillAttributeBufferEC24A((int)Easy3D::E3DAttribute_Color, colors.data());
}
