// +-------------------------------- 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 "BenanoGrabber.h"
#include "BenanoDialog.h"
#include <chrono>
#include <thread>

constexpr float BENANO_UNDEF_FLOAT = -99999.f;
constexpr auto BENANO_CALIB_PATH = "C:\\Program Files\\Benano\\CalibrationFiles\\";

using namespace benano_scansdk;

BenanoGrabber::BenanoGrabber() : started(false), ended(false), locked(false), frameCounter(0)
{
  try
  {
    scanManager = new CScanManager();
    // open device
    CInitialParam initParam;
    initParam.MovableDeviceType = eSDKMovableDeviceType::SDK_NoDevice;
    initParam.CalibrationFilePath = BENANO_CALIB_PATH;
    initParam.MaxSolverCnt = 1;
    scanManager->InitialScanner(initParam);

    if (!scanManager->IsConnected())
    {
      scanManager->Reconnect();
      if (!scanManager->IsConnected())
      {
        ::MessageBox(NULL, CString("Benano scanner not connected"), _T("Initialize"), MB_OK | MB_ICONSTOP);
        return;
      }
    }

    // Enable Zmap generation
    CParameterCtrl& paramController = scanManager->GetParameterController();
    CPostProcessParam* postProcParam = paramController.GetPostProcessParam();
    postProcParam->Data.ZMap_bEnableZMapGeneration = true;
    paramController.ApplyPostProcessParam(postProcParam);

    // Enable normals and colors computation for point cloud
    CScanParam* scanParam = paramController.GetScanParam();
    getNormals = scanParam->IsEnableNormalCalculation();
    getColors = scanParam->IsEnablePointCloudColorOutput();
    scanParam->SetEnableOutputMesh(false);  // we never use the mesh
    paramController.ApplyScanParam(scanParam);

    CLiveCtrl* liveCtrl = &(scanManager->GetLiveController());
    exposureMode = liveCtrl->GetExposureMode();
    brightness = liveCtrl->GetBrightness();
    cameraGain =  liveCtrl->GetCameraGain();

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

BenanoGrabber::~BenanoGrabber()
{
  scanManager->ReleaseScanner();
}

void BenanoGrabber::Start(int num_frame)
{
  if (started)
  {
    ::MessageBox(NULL, _T("A previous scan is currently running, please wait for it to finish"), _T("Start scan"), MB_OK | MB_ICONSTOP);
    return;
  }
  try
  {
    CScanCtrl& scanCtrl = scanManager->GetScanController();

    //1) Set scan result callback
    CScanResultCallback rltCallback;
    rltCallback.SetViewDataResultCallback(this, OnViewDataResultCallback);

    started = true;
    ended = false;

    //2) Create scan background thread
    scanCtrl.StartScan(rltCallback);
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Start scan"), MB_OK | MB_ICONSTOP);
  }
}

long long int BenanoGrabber::WaitFrame(int timeout_ms)
{
  try
  {
    while (!ended)  // wait 10ms for the scan callback to be called
      std::this_thread::sleep_for(std::chrono::milliseconds(10));

    // Get a capture
    locked = true;
    frameCounter++;
    return frameCounter;
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("WaitFrame"), MB_OK | MB_ICONSTOP);
  }
  return 0;
}

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

void BenanoGrabber::Stop()
{
  CScanCtrl& scanCtrl = scanManager->GetScanController();
  scanCtrl.StopScan();
  started = false;
}

int BenanoGrabber::GetBitDepth()
{
  return 32;
}

void BenanoGrabber::GetDepthMap(Easy3D::EDepthMap& map)
{
  // We cannot retrieve DepthMap from Benano's SDK
}

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

void BenanoGrabber::GetZMap(Easy3D::EZMap& zmapArg)
{
  if (!locked)
    throw(std::runtime_error("Frame not available"));
  if (zmapArg.GetType() != EImageType_Depth32f)
    throw(std::runtime_error("Incompatible Depth Map format"));
  (Easy3D::EZMap32f&)zmapArg = zmap;
}

void BenanoGrabber::ConfigureDevice(bool updateParams)
{
  try
  {
    bool worked = true;
    CLiveCtrl* liveCtrl = &(scanManager->GetLiveController());
    worked = worked && liveCtrl->SetExposureMode(exposureMode);
    worked = worked && liveCtrl->SetBrightness(brightness);
    worked = worked && liveCtrl->SetCameraGain(cameraGain);

    if (!worked)
      ::MessageBox(NULL, _T("Please check the scanner is connected"), _T("Configure scanner"), MB_OK | MB_ICONSTOP);

    if (updateParams)
    {
      CParameterCtrl& paramController = scanManager->GetParameterController();
      CScanParam* scanParam = paramController.GetScanParam();
      scanParam->SetEnableNormalCalculation(getNormals);
      scanParam->SetEnablePointCloudColorOutput(getColors);
      paramController.ApplyScanParam(scanParam);
    }
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("Configure scanner"), MB_OK | MB_ICONSTOP);
  }

}

void BenanoGrabber::InvokeParameterPanel()
{
  BenanoParamDialog paramDialog(exposureMode, brightness, cameraGain, getNormals, getColors);

  if (paramDialog.DoModal() == IDOK)
  {
    exposureMode = paramDialog.exposureMode_;
    brightness = paramDialog.brightness_;
    cameraGain = paramDialog.cameraGain_;
    bool updateParams = getNormals != paramDialog.getNormals_ || getColors != paramDialog.getColors_;
    getNormals = paramDialog.getNormals_;
    getColors = paramDialog.getColors_;
    ConfigureDevice(updateParams);
  }
}

void BenanoGrabber::OnViewDataResultCallback(void* cbOwner, CViewDataResult* viewResult)
{
  reinterpret_cast<BenanoGrabber*>(cbOwner)->OnViewDataResult(viewResult);
}

// CViewDataResult will die outside of the callback so we convert the data to EPointCloud/Zmap here
// we retrieve normals and colors from the cloud as well
void BenanoGrabber::OnViewDataResult(CViewDataResult* viewResult)
{
  pointCloud.Clear();
  zmap.Clear();

  // get cloud points
  std::vector<Easy3D::E3DPoint> buffer;
  buffer.reserve(viewResult->PointCloudResult.PointsCount);
  const float* const data = viewResult->PointCloudResult.PointCloud;
  for (size_t i = 0; i < viewResult->PointCloudResult.PointsCount * 3; i += 3)
    buffer.emplace_back(data[i], data[i + 1], data[i + 2]);
  pointCloud.AddPoints(buffer);

  // get normals
  const float* const normalsData = viewResult->PointCloudResult.Normals;
  if (normalsData != nullptr)
  {
    pointCloud.AllocateAttributeBufferE3DPoint(Easy3D::E3DAttribute_Normal, Easy3D::E3DPoint(0.f, 0.f, 0.f));
    Easy3D::E3DPoint* const normals = (Easy3D::E3DPoint*) pointCloud.GetAttributeBuffer(Easy3D::E3DAttribute_Normal);
    for (size_t i = 0; i < viewResult->PointCloudResult.PointsCount; i++)
      normals[i] = Easy3D::E3DPoint(normalsData[3 * i], normalsData[3 * i + 1], normalsData[3 * i + 2]);
  }

  // get colors
  const UINT8* const colorsData = (const UINT8*)viewResult->PointCloudResult.Colors;
  if (colorsData != nullptr)
  {
    pointCloud.AllocateAttributeBufferEC24A(Easy3D::E3DAttribute_Color, EC24A(0u, 0u, 0u, 0u));
    EC24A* const colors = (EC24A*)pointCloud.GetAttributeBuffer(Easy3D::E3DAttribute_Color);
    if (viewResult->PointCloudResult.ColorChannelCount == 1)
    {
      for (size_t i = 0; i < viewResult->PointCloudResult.PointsCount; i++)
        colors[i] = EC24A(colorsData[i], colorsData[i], colorsData[i], 255u);
    }
    else  // ColorChannelCount == 3
    {
      for (size_t i = 0; i < viewResult->PointCloudResult.PointsCount; i++)
        colors[i] = EC24A(colorsData[3 * i], colorsData[3 * i + 1], colorsData[3 * i + 2], 255u);
    }
  }

  // get Zmap
  const CZmapData* const benanoZmap = viewResult->ZmapResult;
  const float* const zmapData = benanoZmap->ImageRaw;
  int width = benanoZmap->ImageWidth;
  int height = benanoZmap->ImageHeight;
  zmap.SetSize(width, height);
  zmap.SetResolution(Easy3D::E3DPoint(float(benanoZmap->ImageResolutionX) / 1000.f,
    float(benanoZmap->ImageResolutionY) / 1000.f, 1.f));

  // benano's zmaps are centered on the center of the image
  // we are centered on the lower left corner of the image
  float biasX = float(width / 2) * zmap.GetXResolution();
  float biasY = float(height / 2) * zmap.GetYResolution();
  Easy3D::E3DTransformMatrix mat = Easy3D::E3DTransformMatrix::CreateTranslationMatrix(-biasX, -biasY, 0.f);
  zmap.SetMapToWorldMatrix(mat);

  const float undef = zmap.GetUndefinedValue().Value;
  for (int i = 0; i < height; i++)
  {
    float*const row = (float*)zmap.GetBufferPtr(0, i);
    for (int j = 0; j < width; j++)
    {
      row[j] = zmapData[width * i + j];
      if (row[j] == BENANO_UNDEF_FLOAT)
        row[j] = undef;
    }
  }

  // callback ended, data is ready
  ended = true;
}
