// +-------------------------------- 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 "IntelRSGrabber.h"
#include "librealsense2/rs_advanced_mode.hpp"

#include "IntelRSDialog.h"

IntelRSGrabber::IntelRSGrabber() : started(false), locked(false), frame_counter(0),
dec_filter(2.f), temp_filter(0.5f, 40.f, 8), min_depth(300), max_depth(700), enable_decimation_filter(false), enable_temporal_filter(false), visual_profile("Short Range")
{
  try
  {
    config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16);
    rs2::pipeline_profile pipeline_profile = pipeline.start(config);
    Sleep(500); // Wait for device to start

    // Retrieve some information
    rs2::device device = pipeline_profile.get_device();
    const char* name = device.get_info(RS2_CAMERA_INFO_NAME); // D415, D435, L515

    // Set depth table and other parameters
    ConfigureSensor();
  }
  catch (const rs2::error & e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Initialize"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Initialize"), MB_OK | MB_ICONSTOP);
  }
}

IntelRSGrabber::~IntelRSGrabber()
{
  pipeline.stop();
}

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

long long int IntelRSGrabber::WaitFrame(int timeout_ms)
{
  try
  {
    if (enable_decimation_filter && enable_temporal_filter)
      frames = pipeline.wait_for_frames(timeout_ms).apply_filter(dec_filter).apply_filter(temp_filter);
    else if (enable_decimation_filter)
      frames = pipeline.wait_for_frames(timeout_ms).apply_filter(dec_filter);
    else if (enable_temporal_filter)
      frames = pipeline.wait_for_frames(timeout_ms).apply_filter(temp_filter);
    else
      frames = pipeline.wait_for_frames(timeout_ms);

    locked = true;
    frame_counter++;

    return frame_counter;
  }
  catch (const rs2::error & e)
  {
    ::MessageBox(NULL, CString(e.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 IntelRSGrabber::ReleaseFrame()
{
  try
  {
    locked = false;
  }
  catch (const rs2::error & e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("ReleaseFrame"), MB_OK | MB_ICONSTOP);
  }
}

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

int IntelRSGrabber::GetBitDepth()
{
  return 16;  // Depth values are always in 16 bits format
}

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

  rs2::depth_frame df = frames.get_depth_frame();

  int w = df.get_width();
  int h = df.get_height();
  int bpp = df.get_bits_per_pixel();
  int stride = df.get_stride_in_bytes();
  const uint8_t* data = (const uint8_t*)df.get_data();

  if (bpp != 16)
    throw(std::runtime_error("Only 16 bits depth map supported"));

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

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

  for (int y = 0; y < h; ++y)
  {
    const uint8_t* src = data + (y*stride);
    void* dst = map.GetBufferPtr(0, y);
    memcpy(dst, src, 2 * w);
  }
}

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

  rs2::depth_frame df = frames.get_depth_frame();
  rs2::pointcloud src_pc;
  rs2::points points = src_pc.calculate(df);

  size_t size = points.size();
  const rs2::vertex* vertices = points.get_vertices();

  // Search x,y,z extent
  rs2::vertex extent = *vertices;
  for (size_t i = 1; i < size; ++i)
  {
    if (extent.x < vertices[i].x)
      extent.x;
    if (extent.y < vertices[i].y)
      extent.y;
    if (extent.z < vertices[i].z)
      extent.z;
  }

  // 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(size);
  Easy3D::E3DPoint p;
  for (size_t i = 0; i < size; ++i, vertices++)
  {
    // (0,0,0) means no data (the origin is the center of the camera)
    if (vertices->x != 0.f || vertices->y != 0.f || vertices->z != 0.f)
    {
      // Convert from meters to millimeters.
      // Switch X/Y, invert Z.
      p.X = vertices->x * 1000.f;
      p.Y = (extent.y - vertices->y) * 1000.f;
      p.Z = (extent.z - vertices->z) * 1000.f;
      pts.push_back(p);
    }
  }

  point_cloud.Clear();
  point_cloud.AddPoints(pts);

  pc = point_cloud;
}

void IntelRSGrabber::GetZMap(Easy3D::EZMap& zmap)
{
  // Intel Realsense SDK cannot generate a ZMap
  // Use Easy3D::EPointCloudToZMapConverter instead

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

void IntelRSGrabber::ConfigureSensor()
{
  rs2::pipeline_profile pipeline_profile = pipeline.get_active_profile();
  rs2::device device = pipeline_profile.get_device();

  // Go over the device's sensors
  for (rs2::sensor& sensor : device.query_sensors())
  {
    // Check if the sensor is a depth sensor
    if (rs2::depth_sensor depth_sensor = sensor.as<rs2::depth_sensor>())
    {
      // Set Visual Preset for L515
      std::vector<rs2_option> opts = depth_sensor.get_supported_options();
      for (rs2_option& opt : opts)
      {
        const char* name = depth_sensor.get_option_name(opt);
        const char* desc = depth_sensor.get_option_description(opt);
        rs2::option_range range = depth_sensor.get_option_range(opt);

        if (strcmp(name, "Visual Preset") == 0)
        {
          for (float v = range.min; v <= range.max; v += range.step)
          {
            const char* v_desc = depth_sensor.get_option_value_description(opt, v);

            if (strcmp(v_desc, visual_profile.c_str()) == 0)
            {
              sensor.set_option(opt, v);
            }
          }
        }
      }

      // Set custom depth table for D415/D435
      if (sensor.supports(RS2_CAMERA_INFO_ADVANCED_MODE))
      {
        const char* mode = sensor.get_info(RS2_CAMERA_INFO_ADVANCED_MODE);

        if (*mode == 'Y')
        {
          rs400::advanced_mode adv_mode(device);

          STDepthTableControl depth_table = adv_mode.get_depth_table();
          depth_table.depthClampMin = min_depth;
          depth_table.depthClampMax = max_depth;

          adv_mode.set_depth_table(depth_table);
        }
      }
    }
  }
}

void IntelRSGrabber::InvokeParameterPanel()
{
  IntelParamDialog param_dialog(min_depth, max_depth, enable_decimation_filter, enable_temporal_filter, visual_profile);

  if (param_dialog.DoModal() == IDOK)
  {
    min_depth = param_dialog.min_depth;
    max_depth = param_dialog.max_depth;
    enable_decimation_filter = param_dialog.enable_decimation_filter;
    enable_temporal_filter = param_dialog.enable_temporal_filter;
    visual_profile = param_dialog.visual_profile;

    ConfigureSensor();
  }
}
