// +-------------------------------- 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 "SSZNGrabber.h"
#include "SSZNDialog.h"
#include <vector>
#include <sstream>
#include <limits>


static void SplitString(const std::string& str, char delim, std::vector<std::string>& result)
{
  std::stringstream ss(str);
  std::string item;

  while (getline(ss, item, delim))
  {
    result.push_back(item);
  }
}

SSZNGrabber::SSZNGrabber(const std::string& device_name) : GrabberInterface(device_name),
started(false), locked(false), frame_counter(0), width(0), height(0),
deviceID(0), dataObject(NULL), heightData(nullptr), camera_config(""),
profiles_distance(1.f)
{
  SR7IF_ETHERNET_CONFIG ethernetConfig;

  std::vector<std::string> vecStrIP;
  SplitString(device_name, '.', vecStrIP);

  if (vecStrIP.size() != 4)
  {
    throw(std::runtime_error("Invalid IP"));
  }

  ethernetConfig.abyIpAddress[0] = (byte)atoi(vecStrIP[0].c_str());
  ethernetConfig.abyIpAddress[1] = (byte)atoi(vecStrIP[1].c_str());
  ethernetConfig.abyIpAddress[2] = (byte)atoi(vecStrIP[2].c_str());
  ethernetConfig.abyIpAddress[3] = (byte)atoi(vecStrIP[3].c_str());

  if (SR7IF_EthernetOpen((unsigned int)(deviceID), &ethernetConfig) != 0)
  {
    throw(std::runtime_error("Ethernet open error"));
  }
}

SSZNGrabber::~SSZNGrabber()
{
  if (heightData != nullptr)
  {
    delete[] heightData;
  }

  SR7IF_CommClose(deviceID);
}

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

  int timeout = 20000;
  SR7IF_StartMeasure(deviceID, timeout);

  started = true;
}

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

  locked = true;

  dataObject = NULL;

  if (SR7IF_ReceiveData(deviceID, dataObject) != 0)
  {
    throw(std::runtime_error("ReceiveData error"));
  }

  // number of profiles
  height = SR7IF_ProfilePointCount(deviceID, dataObject);
  // profile width
  width = SR7IF_ProfileDataWidth(deviceID, dataObject);

  int nbPoint = height * width;

  // Retrieve Z-map
  if (heightData != nullptr)
  {
    delete[] heightData;
  }
  heightData = new int[nbPoint];

  if (SR7IF_GetProfileData(deviceID, dataObject, heightData) != 0)
  {
    throw(std::runtime_error("GetProfileData error"));
  }

  ConvertHeightDataToZMap();

  ++frame_counter;
  return frame_counter;
}

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

  locked = false;
}

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

  if (SR7IF_StopMeasure(deviceID) != 0)
  {
    throw(std::runtime_error("Received data error"));
  }

  started = false;
}

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

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

  // No depthmap available
}

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

  // Convert zmap to pointcloud
  Easy3D::EZMapToPointCloudConverter converter;
  dst_pc.Clear();
  converter.Convert(&zmap, dst_pc);
}

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

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

  Easy3D::EZMap16& dst_zmap16 = dynamic_cast<Easy3D::EZMap16&>(dst_zmap);

  dst_zmap16 = zmap;
}

void SSZNGrabber::InvokeParameterPanel()
{
  SSZNParamDialog param_dialog(camera_config, profiles_distance);

  if (param_dialog.DoModal() == IDOK)
  {
    camera_config = param_dialog.camera_config;
    profiles_distance = param_dialog.profiles_distance;

    // Read the configuration file
    int filesize;
    char* paramBuffer = ReadConfigurationFile(camera_config, filesize);

    if (paramBuffer != nullptr)
    {
      // Set the parameters
      if (SR7IF_LoadParameters(deviceID, paramBuffer, filesize) != 0)
      {
        throw(std::runtime_error("Error while loading parameters"));
      }

      delete[] paramBuffer;
    }
  }
}


void SSZNGrabber::ConvertHeightDataToZMap()
{
  // Convert height data to zmap.

  zmap.SetSize(width, height);

  float rx = float(SR7IF_ProfileData_XPitch(deviceID, dataObject)) / 1000.f;
  float ry = profiles_distance / 1000.f;
  float rz = 0.001f; // 1 mm per pixel
  zmap.SetResolution(rx, ry, rz);

  int i = 0;
  for (int y = 0; y < height; ++y)
  {
    uint16_t* dst = (uint16_t*)zmap.GetBufferPtr(0, y);

    // Copy height values to zmap
    for (int x = 0; x < width; ++x, ++i)
    {
      dst[x] = uint16_t(heightData[i] / 100000); // transform value in mm
    }
  }
}

/*
 * Returns a pointer to a buffer containing the configuration and its size in fileSize.
 * The returned buffer must be freed.
 */
char* SSZNGrabber::ReadConfigurationFile(const std::string strPath, int& fileSize)
{
  CString cstrPath(strPath.c_str());
  DWORD dwNum = WideCharToMultiByte(CP_OEMCP, NULL, cstrPath, -1, NULL, NULL, 0, NULL);

  char *filePath = new char[dwNum];
  WideCharToMultiByte(CP_OEMCP, NULL, cstrPath, -1, filePath, dwNum, 0, NULL);

  fileSize = 0;
  CFileStatus fileStatus;
  if (CFile::GetStatus(cstrPath, fileStatus))
  {
    fileSize = (int)fileStatus.m_size;
  }
  else
  {
    delete[]  filePath;
    throw(std::runtime_error("Error while reading configuration file"));
  }

  char* paramBuffer = new char[fileSize];

  WCHAR wszClassName[256];
  memset(wszClassName, 0, sizeof(wszClassName));
  MultiByteToWideChar(CP_ACP, 0, filePath, strlen(filePath) + 1, wszClassName, sizeof(wszClassName) / sizeof(wszClassName[0]));

  HANDLE hFile;
  hFile = CreateFile(wszClassName, GENERIC_READ, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL);

  DWORD ret;
  ReadFile(hFile, paramBuffer, sizeof(char) * fileSize, &ret, NULL);

  delete[]  filePath;

  return paramBuffer;
}
