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

#include "ATDialog.h"
#include "AT/cx/c3d/ZMap.h"
#include "AT/cx/c3d/PointCloud.h"


// Load calibration from camera using internal buffer.
// We first try to load the new XML calibration format from "CalibrationFactory".
// If this file type is not available we try to load the old binary format from "UserData"
cx_status_t downloadCalib(CX_DEVICE_HANDLE hDevice, CX_CALIB_HANDLE& hCalib)
{
  size_t bufferSz = 0;
  cx_status_t status = CX_STATUS_OK;
  cx::Variant val;

  if ((status = cx_getFileInfo(hDevice, CX_FILE_INFO_SIZE, "CalibrationFactory", val)) == CX_STATUS_OK)
  {
    val.get(bufferSz);
    uint8_t* buffer = (uint8_t*)malloc(bufferSz);
    status = cx_downloadFileToBuffer(hDevice, "CalibrationFactory", buffer, &bufferSz);
    if (status == CX_STATUS_OK)
      status = cx_3d_calib_loadFromBuffer(buffer, bufferSz, "", CX_3D_CALIB_FORMAT_AUTO, &hCalib);	// Load default calibration
    free(buffer);
  }

  if (status != CX_STATUS_OK)
  {
    if ((status = cx_getFileInfo(hDevice, CX_FILE_INFO_SIZE, "UserData", val)) == CX_STATUS_OK)
    {
      val.get(bufferSz);
      uint8_t* buffer = (uint8_t*)malloc(bufferSz);
      status = cx_downloadFileToBuffer(hDevice, "UserData", buffer, &bufferSz);
      if (status == CX_STATUS_OK)
        status = cx_3d_calib_loadFromBuffer(buffer, bufferSz, "", CX_3D_CALIB_FORMAT_AUTO, &hCalib);	// Load default calibration
      free(buffer);
    }
  }
  return status;
}

// Update calibration settings with current acquisition settings from camera
// The following calibration parameters need to be updated with device settings:
// CX_3D_PARAM_S_SZ, CX_3D_PARAM_RANGE_SCALE, CX_3D_PARAM_ROI_XXX.
void updateCalibC6(CX_DEVICE_HANDLE hDevice, CX_CALIB_HANDLE hCalib, int regionId)
{
  cx::Variant val;

  // set sensor size parameter from camera (requires for LUT_mode, binary calibration format does not include this parameter)
  std::vector<int64_t> s_size = { 0, 0 };
  cx::checkOk(cx_getParam(hDevice, "SensorWidth", val));
  s_size.at(0) = val;
  cx::checkOk(cx_getParam(hDevice, "SensorHeight", val));
  s_size.at(1) = val;
  val.set(s_size);
  cx::checkOk(cx_3d_calib_set(hCalib, CX_3D_PARAM_S_SZ, val));	//set sensor size in pixel [width, height]

  cx::checkOk(cx_getParam(hDevice, "Scan3dExtractionSelector", val));
  cx_getParamInfo(hDevice, CX_PARAM_INFO_RANGE, "Scan3dExtractionSelector", val);
  // select extraction module belonging to given AOI

  std::vector<int64_t> enumVals;
  cx::checkOk(val.get(enumVals));

  std::vector<int64_t>::iterator it;
  for (it = enumVals.begin(); it != enumVals.end(); it++)
  {
    val.set(*it);
    cx::checkOk(cx_setParam(hDevice, "Scan3dExtractionSelector", val));

    cx::checkOk(cx_getParamInfo(hDevice, CX_PARAM_INFO_ENUM_INT_VALUE, "Scan3dExtractionSource", val));

    if ((int64_t)val == regionId)
    {
      cx::checkOk(cx_getParam(hDevice, "Scan3dCoordinateScale", val));
      val = double(val);
      cx::checkOk(cx_3d_calib_set(hCalib, CX_3D_PARAM_RANGE_SCALE, val));

      cx::checkOk(cx_getParam(hDevice, "Scan3dCoordinateOffset", val));
      cx::checkOk(cx_3d_calib_set(hCalib, CX_3D_PARAM_RANGE_OFFSET, val));
    }
  }

  // set X offset in image space
  cx::checkOk(cx_getParam(hDevice, CX_CAM_IMAGE_OFFSET_X, val));
  cx::checkOk(cx_3d_calib_set(hCalib, CX_3D_PARAM_S_ROI_X, val));

  cx::checkOk(cx_getParam(hDevice, "RegionSelector", val));
  std::string storedRegion = val;

  // select the AOI on the camera we want to read the parameter for
  cx::checkOk(cx_setParam(hDevice, "RegionSelector", cx::Variant(regionId)));

  // set image width, if reverse-x is true set negative
  cx_status_t result;
  result = cx_getParam(hDevice, CX_CAM_IMAGE_REVERSE_X, val);
  bool reverseX = false;
  if (result == CX_STATUS_OK)
    reverseX = ((int)val == 0) ? false : true;			// TRUE = YES = 1, FALSE = NO = 0
  cx::checkOk(cx_getParam(hDevice, CX_CAM_IMAGE_WIDTH, val));
  int width = val;
  if (reverseX)
    width = -width;
  cx::checkOk(cx_3d_calib_set(hCalib, CX_3D_PARAM_S_ROI_W, cx::Variant(width)));

  // set AOI height, if reverse-y is true set negative
  result = cx_getParam(hDevice, CX_CAM_IMAGE_REVERSE_Y, val);
  bool reverseY = false;
  if (result == CX_STATUS_OK)
    reverseY = ((int)val == 0) ? false : true;			// TRUE = YES = 1, FALSE = NO = 0
  cx::checkOk(cx_getParam(hDevice, "Height", val));
  int roiHeight = val;
  if (reverseY)
    roiHeight = -roiHeight;
  cx::checkOk(cx_3d_calib_set(hCalib, CX_3D_PARAM_S_ROI_H, cx::Variant(roiHeight)));

  cx::checkOk(cx_setParam(hDevice, "RegionSelector", cx::Variant(storedRegion)));
}


ATGrabber::ATGrabber(const std::string& ip_address) : GrabberInterface(ip_address), device(NULL), buffer(NULL), bit_depth(0), started(false), locked(false), frame_counter(0),
exposure_time(500), trigger_mode("QuadratureEncoder"), profiles_per_frame(2048), profiles_distance(12.f / 500),
aoi_x(0), aoi_y(0), aoi_width(2048), aoi_height(1024), sensor_plan_distance(10.0f)
{
  // Discover
  std::string filter = "filter://" + ip_address;
  if (cx_dd_findDevices(filter.c_str(), 1000, CX_DD_USE_GEV) != CX_STATUS_OK)
    throw(std::runtime_error("Failed to discover AT device"));
  unsigned int num = 0;
  if (cx_dd_getNumFoundDevices(&num) != CX_STATUS_OK || num == 0)
    throw(std::runtime_error("Failed to discover AT device, check ip address"));


  // Get information
  cx::Variant val;
  if (cx_dd_getParam(0, CX_CAM_DD_DEVICE_URI, val) != CX_STATUS_OK || val.get(uri))
    throw(std::runtime_error("Failed to discover AT device"));

  if (cx_dd_getParam(0, CX_CAM_DD_DEVICE_MODEL, val) != CX_STATUS_OK || val.get(model) != CX_STATUS_OK)
    throw(std::runtime_error("Failed to discover AT device"));

  // Open device
  device = cx::DeviceFactory::openDevice(uri);

  // Configure
  device->getParam("DeviceScanType", val);
  if (std::string(val) != "Linescan3D")
  {
    // Current sensor device mode is not 'Linescan3D', we activate it.
    device->setParam("DeviceScanType", "Linescan3D");
  }

  device->getParam("PixelFormat", val);
  bit_depth = (std::string(val) == "Mono16" ? 16 : 8);

  // enable multi-part
  cx::checkOk(cx_setParam(device->getHandle(), "GevStreamChannelSelector", cx::Variant(0)));
  cx::checkOk(cx_setParam(device->getHandle(), "GevSCCFGMultiPart", cx::Variant(true)));

  // Get calibration model from device
  downloadCalib(device->getHandle(), calib);

  // Set relevant acquisition parameters in calibration, we will use Region0/Scan3dExtraction0
  updateCalibC6(device->getHandle(), calib, 0);

  // Set the profile-step-width scaling
  calib.setParam(CX_3D_PARAM_SY, profiles_distance);

  // Set the invalid value that will be used in the point cloud for all range map values that are zero
  float ivd = NAN;	// Could be any number (e.g. -10.0f). If NAN is used, the points are automatically suppressed in showPointCloud
  calib.setParam(CX_3D_PARAM_METRIC_IDV, ivd);

  // Trigger cache update. When calling with argument > 0, the internal cache gets updated.
  // This prevents a cache update during the first call to metric function cx_3d_range2calibratedABC.
  calib.setParam(CX_3D_PARAM_METRIC_CACHE_MODE, int(1));
}

ATGrabber::~ATGrabber()
{
  device->close();
  cx::DeviceFactory::closeAllDevices();
}

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

  try
  {
    cx::Variant val;
    val = exposure_time;
    device->setParam("ExposureTime", val);

    val = trigger_mode;
    if (trigger_mode == "QuadratureEncoder")
    {
      device->setParam("EncoderSourceA", "Line2");
      device->setParam("EncoderSourceB", "Line3");
      device->setParam("EncoderMode", "FourPhase");
      device->setParam("EncoderOutputMode", "DirectionUp");
      device->setParam("TriggerSelector", "LineStart");
      device->setParam("TriggerMode", "On");
      device->setParam("TriggerSource", "Encoder0");
    }
    else if (trigger_mode == "FreeRun")
    {
      device->setParam("TriggerSelector", "LineStart");
      device->setParam("TriggerMode", "Off");
    }
    else
      throw(std::runtime_error("ATGrabber::Start: invalid trigger mode selected"));

    // modify region of interest
    device->setParam("RegionSelector", "Region0");
    device->setParam("RegionMode", "On");


    // Reset offset before changing ROI size
    device->setParam("OffsetX", 0);
    device->setParam("OffsetY", 0);

    // Update ROI
    val = aoi_width;
    device->setParam("Width", val);

    val = aoi_height;
    device->setParam("Height", val);

    val = aoi_x;
    device->setParam("OffsetX", val);

    val = aoi_y;
    device->setParam("OffsetY", val);

    // update number of profiles
    device->setParam("RegionSelector", "Scan3dExtraction0");
    device->setParam("RegionMode", "On");
    val = profiles_per_frame;  // aoi_height
    device->setParam("Height", val);

    // Allocate and queue internal acquisition buffers
    device->allocAndQueueBuffers();

    // Start image acquisition
    device->startAcquisition();

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

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

  try
  {
    // Grab acquisition buffer, wait for valid buffer with optional timeout. Timeout is given in ms.
    buffer = device->waitForBuffer(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 ATGrabber::ReleaseFrame()
{
  if (!locked)
    throw(std::runtime_error("ATGrabber::ReleaseFrame: frame not locked"));

  try
  {
    // Queue back the buffer to the device acquisition engine.
    buffer.queueBuffer();

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

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

  try
  {
    device->stopAcquisition();

    device->freeBuffers();

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

int ATGrabber::GetBitDepth()
{
  return bit_depth;
}

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

  auto image = buffer.getImage();
  unsigned int w = image->width();
  unsigned int h = image->height();
  unsigned int pf = image->pixelFormat();

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

    // Copy buffer to EDepthMap8
    map.SetSize(w, h);

    for (unsigned int y = 0; y < h; ++y)
    {
      void* src = image->row<void>(y);
      void* dst = map.GetBufferPtr(0, h - y - 1);
      memcpy(dst, src, w);
    }
  }
  else if (pf == CX_PF_MONO_16)
  {
    if (map.GetType() != EImageType_Depth16)
      throw(std::runtime_error("Incompatible Depth Map format"));

    // Copy buffer to EDepthMap16
    map.SetSize(w, h);

    for (unsigned int y = 0; y < h; ++y)
    {
      void* src = image->row<void>(y);
      void* dst = map.GetBufferPtr(0, h - y - 1);
      memcpy(dst, src, 2 * w);
    }
  }
  else
    throw(std::runtime_error("Unsupported image format"));
}

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

  // Set the profile-step-width scaling
  calib.setParam(CX_3D_PARAM_SY, profiles_distance);

  cx::Point3f offset(0.f, 0.f, 0.f);
  cx::Point3f scale(1.f, 1.f, 1.f);

  calib.setParam(CX_3D_PARAM_METRIC_O, offset);
  calib.setParam(CX_3D_PARAM_METRIC_S, scale);

  // Get the undefined value
  float idv;
  cx::Variant variant;
  calib.getParam(CX_3D_PARAM_METRIC_IDV, variant);
  if (variant.get(idv) != CX_STATUS_OK)
    throw(std::runtime_error("Couldn't retrieve CX_3D_PARAM_METRIC_IDV"));

  // Convert range map to calibrated (X,Y,Z) image
  auto rangeImg = buffer.getImage();
  cx::c3d::PointCloud pc_AT(rangeImg->height(), rangeImg->width());
  cx::c3d::calculatePointCloud(calib, *rangeImg, pc_AT);
  cx::Image& points = pc_AT.points;

  // Push valid 3D points to an EPointCloud
  std::vector<Easy3D::E3DPoint> pts;
  Easy3D::E3DPoint p;
  pts.reserve(points.width() * points.height());
  for (unsigned int y = 0; y < points.height(); ++y)
  {
    float* src = points.row<float>(y);
    for (unsigned int x = 0; x < points.width(); ++x, src += 3)
      if (src[2] != idv)
      {
        p.X = src[0];
        p.Y = src[1];
        p.Z = src[2];
        pts.push_back(p);
      }
  }

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

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

  // Only 16 bits integers are supported
  if (zmap.GetType() != EImageType_Depth16)
    throw(std::runtime_error("Incompatible ZMap format"));

  auto rangeImg = buffer.getImage();
  Easy3D::EZMap16& zmap16 = dynamic_cast<Easy3D::EZMap16&>(zmap);

  // Set the profile-step-width scaling
  calib.setParam(CX_3D_PARAM_SY, profiles_distance);

  float zmap_view_width = 60.f; // mm
  float zmap_view_height = (float)(rangeImg->height()*profiles_distance); // mm

  cx::Point3f offset(-zmap_view_width * 0.5f, 0.0f, -sensor_plan_distance);
  // Set 0.05mm per pixel for X and Y, 0.001mm per grey level (for Z)
  // However, the scale for Y should be higher than the profiles_distance
  float scaleXY = 0.05f > profiles_distance ? 0.05f : profiles_distance;
  cx::Point3f scale(scaleXY, scaleXY, 0.001f); // It is not mandatory to have the same scale for X and for Y.

  calib.setParam(CX_3D_PARAM_METRIC_O, offset);
  calib.setParam(CX_3D_PARAM_METRIC_S, scale);

  // Calculate the rectified image (Z-Map)
  cx::c3d::ZMap zMap(int(zmap_view_height / scale.y), int(zmap_view_width / scale.x), CX_PF_COORD3D_C16, scale, offset);
  cx::c3d::calculateZMap(calib, *rangeImg, zMap);
  cx::Image& rectifiedImg = zMap.img;

  // Set the ZMap resolution
  zmap16.SetResolution(Easy3D::E3DPoint(scale.x, scale.y, scale.z));

  // Copy the pixels to the ZMap16 (the undefined pixel value is the same: 0)
  unsigned int h = rectifiedImg.height();
  unsigned int w = rectifiedImg.width();
  zmap16.SetSize(w, h);
  for (unsigned int y = 0; y < h; ++y)
  {
    uint16_t* src = rectifiedImg.row<uint16_t>(y);
    uint16_t* dst = (uint16_t*)(zmap.GetBufferPtr(0, h - y - 1));
    memcpy(dst, src, 2 * w);
  }
}

void ATGrabber::InvokeParameterPanel()
{
  ATParamDialog param_dialog(exposure_time, trigger_mode, profiles_per_frame, profiles_distance, aoi_x, aoi_y, aoi_width, aoi_height, sensor_plan_distance);

  if (param_dialog.DoModal() == IDOK)
  {
    exposure_time = param_dialog.exposure_time;
    trigger_mode = param_dialog.trigger_mode;
    profiles_per_frame = param_dialog.profiles_per_frame;
    profiles_distance = param_dialog.profiles_distance;
    aoi_x = param_dialog.aoi_x;
    aoi_y = param_dialog.aoi_y;
    aoi_width = param_dialog.aoi_width;
    aoi_height = param_dialog.aoi_height;
    sensor_plan_distance = param_dialog.sensor_plan_distance;
  }
}
