// +-------------------------------- 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 "HeliotisGrabber.h"
#include "HeliotisDialog.h"
#include <cassert>

using namespace mimic;

HeliotisGrabber::HeliotisGrabber(const std::string& device_name) : GrabberInterface(device_name), started(false),
  locked(false), frame_counter(0u), producer(), camera(), buffer(), zScale(1.f), zOffset(0.f), scan_speed(5.f), scan_range(1.f),
  target_vertical_spacing(3.f), exposure_ratio(1.f), scan_position(1.f), pixel_side_length(12.f), filter_zmap(true)
{
  // 1. tries to find Heliotis device
  Mimic& mime = Mimic::getInstance();
  uint32_t nofProducers = mime.update();

  // Heliotis' producer is called diaphus.cti
  bool found = false;
  for (uint32_t i = 0; (i < nofProducers) && !found; i++)
  {
    if (mime.getProducerName(i).find("diaphus") != std::string::npos)
    {
      producer = mime.openProducer(i);
      found = true;
    }
  }

  if (!found)
    throw(std::runtime_error("Failed to discover Heliotis producer."));

  uint32_t nofDevices = producer.update(1800);
  found = false;
  for (uint32_t i = 0; (i < nofDevices) && !found; i++)
  {
    if (device_name.empty() || (producer.getCameraName(i).compare(device_name) == 0))
    {
      camera = std::make_unique<Mimic_Camera>(producer.openCamera(i));
      found = true;
    }
  }

  if (!found)
    throw(std::runtime_error("Failed to discover Heliotis device."));

  // 2. configure device
  camera->nodeMap.setValue<std::string>("Scan3dExtractionMethod", "AcceleratedCenterOfMassIQCorrection");

  camera->nodeMap.setValue<std::string>("ComponentSelector", "Intensity");
  camera->nodeMap.setValue<bool>("ComponentEnable", true);
  camera->nodeMap.setValue<std::string>("ComponentSelector", "Range");
  camera->nodeMap.setValue<bool>("ComponentEnable", false);
  camera->nodeMap.setValue<std::string>("ComponentSelector", "Reflectance");
  camera->nodeMap.setValue<bool>("ComponentEnable", false);
  camera->nodeMap.setValue<std::string>("ComponentSelector", "Phase");
  camera->nodeMap.setValue<bool>("ComponentEnable", false);

  camera->nodeMap.setValue<bool>("ChunkModeActive", true);
  camera->nodeMap.setValue<std::string>("ChunkSelector", "PartCount");
  camera->nodeMap.setValue<bool>("ChunkEnable", true);
  camera->nodeMap.setValue<std::string>("ChunkSelector", "PartFixpointScaling");
  camera->nodeMap.setValue<bool>("ChunkEnable", true);
  camera->nodeMap.setValue<std::string>("ChunkSelector", "PartFrameNo");
  camera->nodeMap.setValue<bool>("ChunkEnable", true);
  camera->nodeMap.setValue<std::string>("ChunkSelector", "PartType");
  camera->nodeMap.setValue<bool>("ChunkEnable", true);
  camera->nodeMap.setValue<std::string>("ChunkSelector", "ZTagCount");
  camera->nodeMap.setValue<bool>("ChunkEnable", true);
  camera->nodeMap.setValue<std::string>("ChunkSelector", "ZTagValue");
  camera->nodeMap.setValue<bool>("ChunkEnable", true);

  camera->nodeMap.setValue<int64_t>("ExtSimpMaxHWin", 10);
  camera->nodeMap.setValue<bool>("zTagInverter", true);
  camera->nodeMap.setValue<std::string>("LightControllerSelector", "LightController0");
  camera->nodeMap.setValue<std::string>("LightControllerSource", "UserOutput0");
  camera->nodeMap.setValue<float>("LightBrightness", 100.f);
  camera->nodeMap.setValue<std::string>("PeakDetectionMode", "GlobalMax");
  camera->nodeMap.setValue<std::string>("BackgroundSuppression", "On");
  camera->nodeMap.setValue<std::string>("FPNCorrection", "AverageLastFrames");
  camera->nodeMap.setValue<int64_t>("FPNCorrectionNFrames", 8);
  camera->nodeMap.setValue<std::string>("TriggerSelector", "AcquisitionStart");
  camera->nodeMap.setValue<std::string>("TriggerSource", "Software");
  camera->nodeMap.setValue<std::string>("Scan3dScalingMethod", "zTags");
  camera->nodeMap.setValue<std::string>("Scan3dDistanceUnit", "um");
  camera->nodeMap.setValue<std::string>("UserOutputSelector", "UserOutput0");
  camera->nodeMap.setValue<bool>("UserOutputValue", false);

  // initialize stage
  camera->nodeMap.setValue<int64_t>("StageInit", 1);

  // lights light
  camera->nodeMap.setValue<std::string>("UserOutputSelector", "UserOutput0");
  camera->nodeMap.setValue<bool>("UserOutputValue", true);

  // sensor position
  camera->nodeMap.setValue<std::string>("ScanMode", "Down");

  // configure acquisition
  camera->nodeMap.setValue<std::string>("TriggerSelector", "AcquisitionStart");
  camera->nodeMap.setValue<std::string>("TriggerMode", "Off");
  camera->nodeMap.setValue<std::string>("TriggerSelector", "FrameStart");
  camera->nodeMap.setValue<std::string>("TriggerMode", "On");
  camera->nodeMap.setValue<std::string>("TriggerSource", "Software");
  camera->nodeMap.setValue<std::string>("TriggerSelector", "RecordingStart");
  camera->nodeMap.setValue<std::string>("TriggerMode", "On");
  camera->nodeMap.setValue<std::string>("TriggerSource", "Stage");
}

HeliotisGrabber::~HeliotisGrabber()
{
  if (camera != nullptr)
    camera->release();
}

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

  try
  {
    // set user-determined params
    camera->nodeMap.setValue<float>("ScanSpeed", scan_speed);
    camera->nodeMap.setValue<float>("ScanRange", scan_range);
    camera->nodeMap.setValue<float>("TargetVerticalSpacing", target_vertical_spacing);
    camera->nodeMap.setValue<float>("ExposureRatio", exposure_ratio);
    camera->nodeMap.setValue<float>("ScanPosition", scan_position);

    // reads back actual values set
    scan_range = camera->nodeMap.getValue<float>("ScanRange");
    target_vertical_spacing = camera->nodeMap.getValue<float>("ActualVerticalSpacing");

    // retrieve scale and offset
    camera->nodeMap.setValue<std::string>("Scan3dCoordinateSelector", "CoordinateC");
    zScale = camera->nodeMap.getValue<float>("Scan3dCoordinateScale");
    zOffset = camera->nodeMap.getValue<float>("Scan3dCoordinateOffset");

    // launches acquisition
    camera->startAcquisition();
    camera->nodeMap.setValue<std::string>("TriggerSelector", "FrameStart");
    camera->nodeMap.setValue<int64_t>("TriggerSoftware", 1);

    started = true;
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Start"), MB_OK | MB_ICONSTOP);
  }
}

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

  try
  {
    buffer.reset(new Mimic_Buffer(camera->getBuffer(uint32_t(timeout_ms))));

    locked = true;
    frame_counter++;

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

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

  try
  {
    if (buffer != nullptr)
      buffer->release();
    locked = false;
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
}

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

  try
  {
    camera->stopAcquisition();
    started = false;
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Stop"), MB_OK | MB_ICONSTOP);
  }
}

int HeliotisGrabber::GetBitDepth()
{
  return 16;
}

void HeliotisGrabber::GetPointCloud(Easy3D::EPointCloud& pc)
{
  Easy3D::EZMap16 zmap;
  GetZMap(zmap);  // gets called twice because of the design of the application
  Easy3D::EZMapToPointCloudConverter converter;
  converter.Convert(zmap, pc);
}

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

  if (buffer->getNumParts() != 1 || buffer->getPartPixelformat(0) != 17825799)
    throw(std::runtime_error("HeliotisGrabber::GetZMap/PointCloud: buffer does not contain one part composed of mono16 point data"));

  int64_t width = buffer->getPartWidth(0);
  int64_t height = buffer->getPartHeight(0);

  zmap.SetSize((int)width, (int)height);
  zmap.SetZResolution(zScale);

  zmap.SetXResolution(pixel_side_length);
  zmap.SetYResolution(pixel_side_length);

  Easy3D::E3DTransformMatrix mat = Easy3D::E3DTransformMatrix::CreateIdentityMatrix();
  mat.SetValue(3, 2, zOffset);
  zmap.SetMapToWorldMatrix(mat);

  if (zmap.GetType() == EImageType_Depth16)
  {
    size_t xPadding = zmap.GetRowPitch() - (width * 2);
    size_t bufferSize = zmap.GetRowPitch() * height;
    buffer->getPartData((uint16_t*)zmap.GetBufferPtr(), &bufferSize, xPadding, int64_t(0));
    assert(bufferSize == zmap.GetRowPitch() * height);
  }
  else
    throw(std::runtime_error("HeliotisGrabber::GetZMap only zMap16 is supported"));

  if (filter_zmap)
  {
    Easy3D::EZMap16 zmapCopy(dynamic_cast<Easy3D::EZMap16&>(zmap));
    Easy3D::EFilters::RemoveNoise(zmapCopy, dynamic_cast<Easy3D::EZMap16&>(zmap),
      Easy3D::ENoiseRemovalMethod_RelativeDifferenceFromMean, 1, 1.f, 0.25f, true);
  }
}

void HeliotisGrabber::InvokeParameterPanel()
{
  HeliotisParamDialog param_dialog(scan_speed, scan_range, target_vertical_spacing, exposure_ratio, scan_position, pixel_side_length, filter_zmap);

  if (param_dialog.DoModal() == IDOK)
  {
    scan_speed = param_dialog.scan_speed;
    scan_range = param_dialog.scan_range;
    target_vertical_spacing = param_dialog.target_vertical_spacing;
    exposure_ratio = param_dialog.exposure_ratio;
    scan_position = param_dialog.scan_position;
    pixel_side_length = param_dialog.pixel_side_length;
    filter_zmap = param_dialog.filter_zmap;
  }
}
