// +-------------------------------- 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 "ZividGrabber.h"
#include "ZividDialog.h"
#include <vector>
#include <sstream>
#include <limits>


ZividGrabber::ZividGrabber(const std::string& device_name) : started(false), locked(false), frame_counter(0),
camera_config(""), retrive_color(1)
{
  try
  {
    // Connecting to camera
    if (device_name == "")
    {
      camera = sensor.connectCamera();
    }
    else
    {
      camera = sensor.connectCamera(Zivid::CameraInfo::SerialNumber(device_name));
    }

    const auto suggestSettingsParameters = Zivid::CaptureAssistant::SuggestSettingsParameters{
            Zivid::CaptureAssistant::SuggestSettingsParameters::AmbientLightFrequency::none,
            Zivid::CaptureAssistant::SuggestSettingsParameters::MaxCaptureTime{ std::chrono::milliseconds{ 1200 } }
    };
    settings = Zivid::CaptureAssistant::suggestSettings(camera, suggestSettingsParameters);
  }
  catch (const std::exception& e)
  {
    throw(std::runtime_error(Zivid::toString(e)));
  }
}

ZividGrabber::~ZividGrabber()
{}

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

  frame = camera.capture(settings);

  started = true;
}

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

  locked = true;

  ConvertPointCloud();

  ++frame_counter;
  return frame_counter;
}

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

  locked = false;
}

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

  started = false;
}

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

void ZividGrabber::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 ZividGrabber::GetPointCloud(Easy3D::EPointCloud& dst_pc)
{
  if (!locked)
    throw(std::runtime_error("Frame not available"));

  dst_pc = pointcloud;
}

void ZividGrabber::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 ZividGrabber::InvokeParameterPanel()
{
  ZividParamDialog param_dialog(camera_config, retrive_color);

  if (param_dialog.DoModal() == IDOK)
  {
    camera_config = param_dialog.camera_config;
    retrive_color = param_dialog.retrive_color;

    // Set the configuration file
    if (camera_config != "")
    {
      try
      {
        settings = Zivid::Settings(camera_config);
      }
      catch (const std::exception& e)
      {
        std::string s = Zivid::toString(e);
        throw(std::runtime_error(s));
      }
    }
  }
}

void ZividGrabber::ConvertPointCloud()
{
  const auto pointCloudSensor = frame.pointCloud();
  size_t width = pointCloudSensor.width();
  size_t height = pointCloudSensor.height();
  size_t nbPoints = width * height;

  std::vector<Easy3D::E3DPoint> points;
  points.reserve(nbPoints);
  float max = FLT_MIN;

  // Clear Open eVision pointcloud
  pointcloud.Clear();

  if (retrive_color)
  {
    const auto data = pointCloudSensor.copyData<Zivid::PointXYZColorRGBA>();
    std::vector<EC24A> colors;
    colors.reserve(nbPoints);
    for (size_t i = 0; i < nbPoints; ++i)
    {
      if (!data(i).point.isNaN())
      {
        points.emplace_back(data(i).point.x, -data(i).point.y, data(i).point.z);
        colors.emplace_back(data(i).color.r, data(i).color.g, data(i).color.b, data(i).color.a);

        if (data(i).point.z > max)
          max = data(i).point.z;
      }
    }

    // Change Z convention
    for (int i = 0; i < points.size(); ++i)
      points[i].Z = max - points[i].Z;

    pointcloud.AddPoints(points);
    pointcloud.FillAttributeBufferEC24A(Easy3D::E3DAttribute_Color, colors.data());
  }
  else
  {
    const auto data = pointCloudSensor.copyData<Zivid::PointXYZ>();
    for (size_t i = 0; i < nbPoints; ++i)
    {
      if (!data(i).isNaN())
      {
        points.emplace_back(data(i).x, -data(i).y, data(i).z);

        if (data(i).z > max)
          max = data(i).z;
      }
    }

    // Change Z convention
    for (int i = 0; i < points.size(); ++i)
      points[i].Z = max - points[i].Z;

    pointcloud.AddPoints(points);
  }
}
