// +-------------------------------- 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 "IDSEnsensoGrabber.h"
#include "IDSEnsensoDialog.h"

#include <fstream>
#include <climits>

#define BIT_DEPTH 16

static std::string NxLibExceptionString(NxLibException& e)
{
  char buffer1[1024];
  sprintf_s(buffer1, "An NxLib API error with code %d (%s) occurred while accessing item %s.\n", e.getErrorCode(), e.getErrorText().c_str(), e.getItemPath().c_str());
  std::string errorMsg(buffer1);

  if (e.getErrorCode() == NxLibExecutionFailed)
  {
    char buffer2[1024];
    sprintf_s(buffer2, "/Execute:\n%s\n", NxLibItem(itmExecute).asJson(true).c_str());
    errorMsg.append(buffer2);
  }

  return errorMsg;
}

IDSEnsensoGrabber::IDSEnsensoGrabber(const std::string& device_name) : GrabberInterface(device_name),
id(0), started(false), locked(false), frame_counter(0), camera_config("")
{
  try
  {
    // Opening NxLib and waiting for cameras to be detected
    nxLibInitialize(true);

    // Check if the device's id is defined
    if (device_name != "")
      id = stoul(device_name);

    // Create an object referencing the camera's tree item, for easier access
    camera = root[itmCameras][itmBySerialNo][id];
    if (!camera.exists() || camera[itmType] != valStereo)
    {
      if (device_name != "")
        throw(std::runtime_error("Please connect a single stereo camera to your computer or define the device_name"));
      else
        throw(std::runtime_error("No stereo camera found for the id: " + id));
    }

    // Set parameters for the command
    std::string serial = camera[itmSerialNumber].asString();

    // When calling the 'execute' method in this object, it will synchronously execute the command 'cmdOpen'
    NxLibCommand open(cmdOpen);

    // Set parameters for the open command
    open.parameters()[itmCameras] = serial;
    open.execute();
  }
  catch (NxLibException& e)
  {
    throw(std::runtime_error(NxLibExceptionString(e)));
  }
  catch (...)
  {
    throw(std::runtime_error("An error occurred"));
  }
}

IDSEnsensoGrabber::~IDSEnsensoGrabber()
{
  if (started)
    Stop();

  // Close the camera
  try
  {
    NxLibCommand(cmdClose).execute();
    nxLibFinalize();
  }
  catch (...)
  {
    // Do not throw in a destructor
  }
}

// Acquisition control

void IDSEnsensoGrabber::Start(int num_frame)
{
  if (started)
    throw(std::runtime_error("IDSEnsensoGrabber::Start: grabber already started"));
  else
    started = true;

  try
  {
    NxLibCommand(cmdCapture).execute();
  }
  catch (NxLibException& e)
  {
    started = false;
    throw(std::runtime_error(NxLibExceptionString(e)));
  }
  catch (...)
  {
    started = false;
    throw(std::runtime_error("An error occurred while capturing a frame."));
  }
}

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

  try
  {
    // This is the actual, computation intensive stereo matching task
    NxLibCommand(cmdComputeDisparityMap).execute();

    locked = true;
    frame_counter++;
  }
  catch (NxLibException& e)
  {
    started = false;
    throw(std::runtime_error(NxLibExceptionString(e)));
  }
  catch (...)
  {
    started = false;
    throw(std::runtime_error("An error occurred while waiting for a frame."));
  }

  return frame_counter;
}

void IDSEnsensoGrabber::ReleaseFrame()
{
  locked = false;
}

void IDSEnsensoGrabber::Stop()
{
  if (!started)
    throw(std::runtime_error("IDSEnsensoGrabber::Stop: grabber not started"));
  else
    started = false;

  try
  {
    // Force to stop cmdCapture if it is still running
    NxLibCommand(cmdBreak).execute();
  }
  catch (NxLibException& e)
  {
    throw(std::runtime_error(NxLibExceptionString(e)));
  }
  catch (...)
  {
    throw(std::runtime_error("An error occurred while stopping the capture of a frame."));
  }
}

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

// Data retrieval

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

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

  NxLibItem dispMap = camera[itmImages][itmDisparityMap];

  int width, height;
  dispMap.getBinaryDataInfo(&width, &height, 0, 0, 0, 0);

  std::vector<int16_t> binaryData;
  dispMap.getBinaryData(binaryData, 0);

  if (width * height != binaryData.size())
    throw(std::runtime_error("Unexpected data size"));

  // convert from signed short to unsigned short
  // search for the minimum value
  int d_min = 32767;
  for (int i = 0; i < binaryData.size(); ++i)
  {
    int16_t d = binaryData[i];

    if (d > -32768 && d < d_min)
      d_min = d;
  }

  // Copy buffer to EDepthMap16
  map.SetSize(width, height);

  int i = 0;
  for (int y = 0; y < height; ++y)
  {
    uint16_t* dst = (uint16_t*)map.GetBufferPtr(0, y);

    // Copy disparity values to depth buffer z-values
    for (int x = 0; x < width; ++x, ++i)
    {
      int d = binaryData[i];

      if (d > -32768) // skip undefined pixel
        dst[x] = uint16_t(d - d_min);
      else
        dst[x] = 0;
    }
  }
}

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

  // This converts the disparity map into XYZ data for each pixel
  NxLibCommand(cmdComputePointMap).execute();

  // Get info about the computed point map and copy it into a std::vector
  int width, height;
  camera[itmImages][itmPointMap].getBinaryDataInfo(&width, &height, 0, 0, 0, 0);

  std::vector<float> pointMap;
  camera[itmImages][itmPointMap].getBinaryData(pointMap, 0);

  int size = width * height;

  // Push valid 3D points to an EPointCloud
  // with coordinate system flipping
  std::vector<Easy3D::E3DPoint> pts;
  pts.reserve(size);
  float max = -1.0f;

  Easy3D::E3DPoint p;
  for (int i = 0; i < size; ++i)
  {
    p.X = pointMap[3 * i];
    if (!std::isnan(p.X))
    {
      p.Y = height - pointMap[3 * i + 1];
      p.Z = pointMap[3 * i + 2];

      pts.push_back(p);
      if (p.Z > max)
        max = p.Z;
    }
  }

  for (int i = 0; i < pts.size(); ++i)
    pts[i].Z = max - pts[i].Z;

  // Update the point cloud
  point_cloud.Clear();
  point_cloud.AddPoints(pts);

  pc = point_cloud;
}

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

  // IDS Ensenso SDK does not generate a ZMap
  // Use Easy3D::EPointCloudToZMapConverter instead

  Easy3D::EPointCloudToZMapConverter converter;
  converter.Convert(point_cloud, &zmap);
}

// Parameter handling GUI

void IDSEnsensoGrabber::InvokeParameterPanel()
{
  IDSEnsensoParamDialog param_dialog(camera_config);

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

  // Set the camera parameters
  try
  {
    // Load json config
    if (camera_config != "")
    {
      std::ifstream file(camera_config.c_str());
      if (file.is_open() && file.rdbuf())
      {
        std::stringstream buffer;
        buffer << file.rdbuf();
        std::string const& fileContent = buffer.str();

        /*
        We now have the entire json representation of the parameter set in the string variable 'fileContent'.
        In order to detect if the file contains the parameters only, or the entire camera node we simply write it
        into a temporary NxLib node "/tmp" and check if it contains a subitem 'Parameters'; if it does, we only use
        this subitem to rewrite the camera's parameter tree, otherwise we assume that the file contained the
        parameter subtree only
        */
        NxLibItem tmp("/tmp");
        tmp.setJson(fileContent); // Parse json file content into temporary item

        if (tmp[itmParameters].exists())
        {
          // Looks like we had a file containing the full camera node; let's just use the parameters subtree to
          // overwrite the camera's parameter subtree
          camera[itmParameters].setJson(tmp[itmParameters].asJson(), true); // writeableNodesOnly = true silently skips
                                                                            // read-only nodes instead of failing when
                                                                            // encountering a read-only node
        }
        else
        {
          // It seems that our file only contained the parameters node, because we don't have Parameters as a subnode;
          // We then use the entire content of the temporary node to rewrite the camera's parameters node
          camera[itmParameters].setJson(tmp.asJson(), true); // with writebleNodesOnly = true, see comment above
        }
      }
      else
      {
        // File could not be read
        throw(std::runtime_error("The Json configuration file could not be read."));
      }
    }
  }
  catch (NxLibException& e)
  {
    throw(std::runtime_error(NxLibExceptionString(e)));
  }
  catch (...)
  {
    throw(std::runtime_error("An error occurred while stopping the capture of a frame."));
  }
}