// +-------------------------------- 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 "LucidHeliosGrabber.h"

#include "LucidHeliosDialog.h"

LucidHeliosGrabber::LucidHeliosGrabber() : system(NULL), device(NULL), image(NULL), coordA_scale(1.0), coordB_scale(1.0), coordC_scale(1.0),
started(false), locked(false), frame_counter(0),
exposure_time_selector("Exp250Us"), operating_mode("Distance1500mm"), spatial_filter_enable(false), image_accumulation(1),
min_depth(0), max_depth(65535)
{
  try
  {
    // Initialize
    system = Arena::OpenSystem();
    if (!system->UpdateDevices(1000))
      throw(std::runtime_error("No device found"));

    std::vector<Arena::DeviceInfo> deviceInfos = system->GetDevices();
    if (deviceInfos.size() == 0)
      throw(std::runtime_error("No camera connected"));

    device = system->CreateDevice(deviceInfos[0]);

    // Set acquisition mode
    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "AcquisitionMode", "Continuous");

    // Set buffer handling mode
    Arena::SetNodeValue<GenICam::gcstring>(device->GetTLStreamNodeMap(), "StreamBufferHandlingMode", "NewestOnly");

    // Enable stream to auto negotiate packet size
    Arena::SetNodeValue<bool>(device->GetTLStreamNodeMap(), "StreamAutoNegotiatePacketSize", true);

    // Enable stream packet resend
    Arena::SetNodeValue<bool>(device->GetTLStreamNodeMap(), "StreamPacketResendEnable", true);

    // Set pixel format
    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "PixelFormat", "Coord3D_ABC16");

    // Get coordinate scales
    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "Scan3dCoordinateSelector", "CoordinateA");
    coordA_scale = (float)Arena::GetNodeValue<double>(device->GetNodeMap(), "Scan3dCoordinateScale");

    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "Scan3dCoordinateSelector", "CoordinateB");
    coordB_scale = (float)Arena::GetNodeValue<double>(device->GetNodeMap(), "Scan3dCoordinateScale");

    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "Scan3dCoordinateSelector", "CoordinateC");
    coordC_scale = (float)Arena::GetNodeValue<double>(device->GetNodeMap(), "Scan3dCoordinateScale");

    // Setup default user parameters
    UpdateUserParameters();

    // start continuous acquisition
    device->StartStream();
  }
  catch (GenICam::GenericException& ge)
  {
    ::MessageBox(NULL, CString(ge.what()), _T("Initialize"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Initialize"), MB_OK | MB_ICONSTOP);
  }
}

LucidHeliosGrabber::~LucidHeliosGrabber()
{
  try
  {
    // Stop acquisition
    device->StopStream();

    // Clean up
    system->DestroyDevice(device);
    Arena::CloseSystem(system);
  }
  catch (GenICam::GenericException& ge)
  {
    ::MessageBox(NULL, CString(ge.what()), _T("Start"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Start"), MB_OK | MB_ICONSTOP);
  }
}

void LucidHeliosGrabber::Start(int num_frame)
{
  started = true;
}

long long int LucidHeliosGrabber::WaitFrame(int timeout_ms)
{
  try
  {
    // Get image
    image = device->GetImage(timeout_ms);

    if (image->IsIncomplete())
      return 0;

    // Get image information
    uint64_t timestampNs = image->GetTimestampNs();
    uint64_t id = image->GetFrameId();

    locked = true;
    frame_counter++;

    return frame_counter;
  }
  catch (GenICam::GenericException& ge)
  {
    ::MessageBox(NULL, CString(ge.what()), _T("WaitFrame"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("WaitFrame"), MB_OK | MB_ICONSTOP);
  }
  return 0;
}

void LucidHeliosGrabber::ReleaseFrame()
{
  try
  {
    // Requeue image buffer
    device->RequeueBuffer(image);

    locked = false;
  }
  catch (GenICam::GenericException& ge)
  {
    ::MessageBox(NULL, CString(ge.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
}

void LucidHeliosGrabber::Stop()
{
  started = false;
}

int LucidHeliosGrabber::GetBitDepth()
{
  return 16;  // Depth values are always in 16 bits format (Coord3D_ABCY16, Coord3D_ABC16 or Coord3D_C16)
}

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

  int w = (int)image->GetWidth();
  int h = (int)image->GetHeight();
  uint64_t pf = image->GetPixelFormat();
  int pix_size = (int)image->GetBitsPerPixel() / 16;

  if (pf != Coord3D_C16 && pf != Coord3D_ABC16 && pf != Coord3D_ABCY16)
    throw(std::runtime_error("Unsupported image format"));

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

  Easy3D::EDepthMap16& map16 = dynamic_cast<Easy3D::EDepthMap16&>(map);

  // Choose the source origin and pixel size
  const uint16_t* data = (const uint16_t*)image->GetData();

  if (pf == Coord3D_ABC16 || pf == Coord3D_ABCY16)
    data += 2;    // Move source origin to the Coordinate_C

  // Copy buffer to EDepthMap16
  map16.SetSize(w, h);
  uint16_t undef = map16.GetUndefinedValue().Value;
  for (int y = 0; y < h; ++y)
  {
    const uint16_t* src = data + y * w * pix_size;
    uint16_t* dst = (uint16_t*)map16.GetBufferPtr(0, y);
    for (int x = 0; x < w; ++x)
    {
      if (*src != 0x8000 && *src >= min_depth && *src <= max_depth)
        *dst = *src;
      else
        *dst = undef;

      src += pix_size;
      dst++;
    }
  }
}

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

  int w = (int)image->GetWidth();
  int h = (int)image->GetHeight();
  int npix = w * h;
  uint64_t pf = image->GetPixelFormat();
  const int16_t* data = (const int16_t*)image->GetData();
  int pix_size = (int)image->GetBitsPerPixel() / 16;

  if (pf != Coord3D_ABC16 && pf != Coord3D_ABCY16)
    throw(std::runtime_error("Unsupported image format"));

  // Search y and z maxima
  int16_t max_y(-32768), max_z(-32768);
  const int16_t* src = data;
  for (int i = 0; i < npix; ++i, src += pix_size)
  {
    if (src[0] != -32768 && src[2] >= min_depth && src[2] <= max_depth)
    {
      if (src[1] > max_y)
        max_y = src[1];
      if (src[2] > max_z)
        max_z = src[2];
    }
  }

  // Push valid 3D points to an EPointCloud
  // Change the coordinate system to have a "go up" instead of "go down" Z axis
  // (from a camera based system to a ground based system)
  std::vector<Easy3D::E3DPoint> pts;
  pts.reserve(npix);
  Easy3D::E3DPoint p;

  src = data;
  for (int i = 0; i < npix; ++i, src += pix_size)
  {
    if (src[0] != -32768 && src[2] >= min_depth && src[2] <= max_depth)
    {
      p.X = coordA_scale * src[0];
      p.Y = coordB_scale * (max_y - src[1]);
      p.Z = coordC_scale * (max_z - src[2]);
      pts.push_back(p);
    }
  }

  pc.Clear();
  pc.AddPoints(pts);

  point_cloud = pc;
}


void LucidHeliosGrabber::GetZMap(Easy3D::EZMap& zmap)
{
  // Lucid Arena SDK cannot generate a ZMap
  // Use Easy3D::EPointCloudToZMapConverter instead

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

void LucidHeliosGrabber::InvokeParameterPanel()
{
  LucidHeliosDialog param_dialog(exposure_time_selector.c_str(), operating_mode.c_str(), spatial_filter_enable, (int)image_accumulation,
    min_depth, max_depth);

  if (param_dialog.DoModal() == IDOK)
  {
    device->StopStream();

    exposure_time_selector = param_dialog.exposure_time_selector.c_str();
    operating_mode = param_dialog.operating_mode.c_str();
    spatial_filter_enable = param_dialog.spatial_filter_enable;
    image_accumulation = (int64_t)param_dialog.image_accumulation;
    min_depth = param_dialog.min_depth;
    max_depth = param_dialog.max_depth;

    UpdateUserParameters();

    device->StartStream();
  }
}

void LucidHeliosGrabber::UpdateUserParameters()
{
  try
  {
    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "ExposureTimeSelector", exposure_time_selector);
    exposure_time_selector = Arena::GetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "ExposureTimeSelector");

    Arena::SetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "Scan3dOperatingMode", operating_mode);
    operating_mode = Arena::GetNodeValue<GenICam::gcstring>(device->GetNodeMap(), "Scan3dOperatingMode");

    Arena::SetNodeValue<bool>(device->GetNodeMap(), "Scan3dSpatialFilterEnable", spatial_filter_enable);
    spatial_filter_enable = Arena::GetNodeValue<bool>(device->GetNodeMap(), "Scan3dSpatialFilterEnable");

    Arena::SetNodeValue<int64_t>(device->GetNodeMap(), "Scan3dImageAccumulation", image_accumulation);
    image_accumulation = Arena::GetNodeValue<int64_t>(device->GetNodeMap(), "Scan3dImageAccumulation");
  }
  catch (GenICam::GenericException& ge)
  {
    ::MessageBox(NULL, CString(ge.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
}
