// +-------------------------------- 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 "PhotoneoGrabber.h"
#include "PhotoneoDialog.h"

#include <sstream>
#include <limits>

#define BIT_DEPTH 16
#define MIN_SHUTTER 1
#define MAX_SHUTTER 20
#define MIN_SCAN 1
#define MAX_SCAN 50


static void GetDeviceInfo(const pho::api::PhoXiDeviceInformation &DeviceInfo, std::stringstream& ss)
{
  ss << "  Name:                    " << DeviceInfo.Name << std::endl;
  ss << "  Hardware Identification: " << DeviceInfo.HWIdentification << std::endl;
  ss << "  Type:                    " << std::string(DeviceInfo.Type) << std::endl;
  ss << "  Firmware version:        " << DeviceInfo.FirmwareVersion << std::endl;
  ss << "  Variant:                 " << DeviceInfo.Variant << std::endl;
  ss << "  IsFileCamera:            " << (DeviceInfo.IsFileCamera ? "Yes" : "No") << std::endl;
  ss << "  Status:                  "
    << (DeviceInfo.Status.Attached ? "Attached to PhoXi Control. " : "Not Attached to PhoXi Control. ")
    << (DeviceInfo.Status.Ready ? "Ready to connect" : "Occupied")
    << std::endl << std::endl;
}

static void GetDeviceInfoList(const std::vector<pho::api::PhoXiDeviceInformation> &DeviceList, std::stringstream& ss)
{
  for (std::size_t i = 0; i < DeviceList.size(); ++i)
  {
    ss << "Device: " << i << std::endl;
    GetDeviceInfo(DeviceList[i], ss);
  }
}

PhotoneoGrabber::PhotoneoGrabber(const std::string& device_name) : GrabberInterface(device_name),
id(0), started(false), locked(false), frame_counter(0)
{
  pho::api::PhoXiFactory Factory;

  // Check if the PhoXi Control Software is running
  if (!Factory.isPhoXiControlRunning())
  {
    throw(std::runtime_error("PhoXi Control Software is not running."));
  }

  // Get List of available devices on the network
  std::vector <pho::api::PhoXiDeviceInformation> DeviceList = Factory.GetDeviceList();
  if (DeviceList.empty())
  {
    throw(std::runtime_error("PhoXi Factory has found 0 device."));
  }

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

  // Try to connect device opened in PhoXi Control, if any
  PhoXiDevice = Factory.CreateAndConnectFirstAttached();
  if (!PhoXiDevice)
  {
    // No PhoXi device is opened in PhoXi Control, try to connect to the sensor defined by device_name (first one by default)
    PhoXiDevice = Factory.CreateAndConnect(DeviceList[id].HWIdentification);

    // Check if device was created
    if (!PhoXiDevice)
    {
      std::stringstream ss;
      ss << "No PhoXi device is opened in PhoXi Control and impossible to create device with id '" << id << "'." << std::endl;
      ss << "List of devices available:" << std::endl;
      GetDeviceInfoList(DeviceList, ss);

      throw(std::runtime_error(ss.str()));
    }
  }

  // Check if device is connected
  if (!PhoXiDevice->isConnected())
  {
    std::stringstream ss;
    ss << "Cannot connect to device '" << PhoXiDevice->GetName() << "'." << std::endl;
    throw(std::runtime_error(ss.str()));
  }
}

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

  PhoXiDevice->Disconnect();
}

// Acquisition control

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

  if (PhoXiDevice->isAcquiring())
  {
    // Stop acquisition to change trigger mode
    PhoXiDevice->StopAcquisition();
  }

  PhoXiDevice->TriggerMode = pho::api::PhoXiTriggerMode::Software;
  PhoXiDevice->ClearBuffer();
  PhoXiDevice->StartAcquisition();

  if (!PhoXiDevice->isAcquiring())
  {
    throw(std::runtime_error("PhotoneoGrabber::Start: device could not start acquisition."));
  }

  FrameID = PhoXiDevice->TriggerFrame();
  if (FrameID < 0)
  {
    throw(std::runtime_error("PhotoneoGrabber::Start: Trigger was unsuccessful."));
  }
}

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

  Frame = PhoXiDevice->GetSpecificFrame(FrameID, pho::api::PhoXiTimeout::Infinity);

  if (Frame)
  {
    locked = true;
    frame_counter++;
  }
  else
  {
    throw(std::runtime_error("Failed to retrieve the frame."));
  }

  return frame_counter;
}

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

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

  PhoXiDevice->StopAcquisition();
}

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

// Data retrieval

void PhotoneoGrabber::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"));

  if (Frame->DepthMap.Empty())
    throw(std::runtime_error("Cannot retrieve depth map."));

  int width = Frame->DepthMap.Size.Width;
  int height = Frame->DepthMap.Size.Height;

  float *data = (float*)Frame->DepthMap.GetDataPtr();

  map.SetSize(width, height);

  float max = FLT_MIN, min = FLT_MAX;
  int index = 0;
  for (int y = 0; y < height; ++y)
  {
    for (int x = 0; x < width; ++x, ++index)
    {
      if (data[index] > max)
        max = data[index];

      if (data[index] != 0.0f && data[index] < min)
        min = data[index];
    }
  }

  float rangeFactor = 65536.0f / (max - min);

  index = 0;
  for (int y = 0; y < height; ++y)
  {
    // Copy each row of the depth map
    uint16_t *dst = (uint16_t*)map.GetBufferPtr(0, y);
    for (int x = 0; x < width; ++x, ++index)
    {
      if (data[index] == 0.f)
        dst[x] = 0;
      else
      {
        dst[x] = uint16_t((max - data[index]) * rangeFactor);
      }
    }
  }
}

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

  if (Frame->PointCloud.Empty())
    throw(std::runtime_error("Cannot retrieve pointcloud."));

  int width = Frame->PointCloud.Size.Width;
  int height = Frame->PointCloud.Size.Height;
  int size = width * height;
  float max = FLT_MIN;

  float *data = (float*)Frame->PointCloud.GetDataPtr();

  std::vector<Easy3D::E3DPoint> pts;
  pts.reserve(size);

  Easy3D::E3DPoint p;
  for (int i = 0; i < size; ++i)
  {
    p.X = data[3 * i];
    p.Y = -data[3 * i + 1];
    p.Z = data[3 * i + 2];
    if (p.X != 0.0f && p.Y != 0.0f && p.Z != 0.0f)
    {
      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 PhotoneoGrabber::GetZMap(Easy3D::EZMap& zmap)
{
  if (!locked)
    throw(std::runtime_error("Frame not available"));

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

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

// Parameter handling GUI

void PhotoneoGrabber::InvokeParameterPanel()
{
  // Check if the CapturingModes are Enabled and Can be Set
  if (!PhoXiDevice->CapturingMode.isEnabled() || !PhoXiDevice->CapturingMode.CanGet() || !PhoXiDevice->CapturingMode.CanSet() || !PhoXiDevice->SupportedCapturingModes.isEnabled() || !PhoXiDevice->SupportedCapturingModes.CanGet())
  {
    throw(std::runtime_error("Capturing Modes are not supported by the Device Hardware, or are Read only on the specific device"));
  }

  std::string quality = PhoXiDevice->CapturingSettings->CodingQuality;
  int shutter = PhoXiDevice->CapturingSettings->ShutterMultiplier;
  int scan = PhoXiDevice->CapturingSettings->ScanMultiplier;

  pho::api::PhoXiSize resolutionSize = PhoXiDevice->Resolution;
  int resolution;
  if (resolutionSize.Width == 2064)
    resolution = 0;
  else
    resolution = 1;

  PhotoneoParamDialog param_dialog(quality, resolution, shutter, scan);
  if (param_dialog.DoModal() != IDOK)
    return;

  // CodingQuality values: Fast / High / Ultra
  PhoXiDevice->CapturingSettings->CodingQuality = param_dialog.quality;

  //ShutterMultiplier values: 1-20
  if (param_dialog.shutterM < MIN_SHUTTER)
    PhoXiDevice->CapturingSettings->ShutterMultiplier = MIN_SHUTTER;
  else if (param_dialog.shutterM > MAX_SHUTTER)
    PhoXiDevice->CapturingSettings->ShutterMultiplier = MAX_SHUTTER;
  else
    PhoXiDevice->CapturingSettings->ShutterMultiplier = param_dialog.shutterM;

  //ScanMultiplier values: 1-50
  if (param_dialog.scanM < MIN_SCAN)
    PhoXiDevice->CapturingSettings->ScanMultiplier = MIN_SCAN;
  else if (param_dialog.scanM > MAX_SCAN)
    PhoXiDevice->CapturingSettings->ScanMultiplier = MAX_SCAN;
  else
    PhoXiDevice->CapturingSettings->ScanMultiplier = param_dialog.scanM;

  //Resolution values: 0 / 1 (0 is 2064x1544, 1 is 1032x772)
  //Get all supported modes
  std::vector<pho::api::PhoXiCapturingMode> SupportedCapturingModes = PhoXiDevice->SupportedCapturingModes;
  PhoXiDevice->CapturingMode = SupportedCapturingModes[param_dialog.resolution];
}