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

#include "LMIDialog.h"


LMIGrabber::LMIGrabber(const std::string& device_name) : GrabberInterface(device_name),
api(kNULL), system(kNULL), sensor(kNULL), setup(kNULL), dataset(kNULL),
started(false), locked(false), frame_counter(0), point_cloud(NULL), zmap(NULL),
acquisition_mode("ZMap (Uniform Surface)"), exposure_time(EXPOSURE_TIME_DEFAULT), frequency(FREQUENCY_DEFAULT),
x_field_of_view(X_FIELD_OF_VIEW), measurement_range(MEASUREMENT_RANGE), x_start(X_START), z_start(Z_START), surface_length(SURFACE_LENTH)
{
  // construct Gocator API Library
  if (GoSdk_Construct(&api) != kOK)
  {
    throw(std::runtime_error("Failed to construct Gocator API Library"));
  }

  // construct GoSystem object
  if (GoSystem_Construct(&system, kNULL) != kOK)
  {
    throw(std::runtime_error("Failed to construct GoSystem object"));
  }

  // find the sensor
  if (kIpAddress_Parse(&ipAddress, device_name.c_str()) == kOK)
  {
    // obtain GoSensor object by IP address
    if (GoSystem_FindSensorByIpAddress(system, &ipAddress, &sensor) != kOK)
    {
      throw(std::runtime_error("Failed to obtain GoSensor object by IP address"));
    }
  }
  else if ((sensorCount = GoSystem_SensorCount(system)) == 1)
  {
    // obtain the first GoSensor object if there is only one
    sensor = GoSystem_SensorAt(system, 0);
  }
  else if (sensorCount == 0)
  {
    // no GoSensor found
    throw(std::runtime_error("There is no GoSensor connected"));
  }
  else
  {
    id = stoul(device_name);
    // obtain GoSensor object by ID
    if (GoSystem_FindSensorById(system, id, &sensor) != kOK)
    {
      throw(std::runtime_error("Failed to obtain GoSensor object by ID"));
    }
  }

  // create connection to GoSensor object
  if (GoSensor_Connect(sensor) != kOK)
  {
    throw(std::runtime_error("Failed to create connection to GoSensor object"));
  }

  // enable sensor data channel
  if (GoSystem_EnableData(system, kTRUE) != kOK)
  {
    throw(std::runtime_error("Failed to enable sensor data channel"));
  }

  // retrieve setup handle
  if ((setup = GoSensor_Setup(sensor)) == kNULL)
  {
    throw(std::runtime_error("Failed to retrieve setup handle"));
  }

  // Setup the surface mode
  if (GoSetup_SetScanMode(setup, GO_MODE_SURFACE) != kOK)
  {
    throw(std::runtime_error("Failed to setup the surface mode"));
  }

  surface = GoSetup_SurfaceGeneration(setup);

  // Use time as trigger unit
  if (GoSetup_SetTriggerUnit(setup, GO_TRIGGER_UNIT_TIME) != kOK)
  {
    throw(std::runtime_error("Failed to set time as trigger unit"));
  }
}

LMIGrabber::~LMIGrabber()
{
  delete point_cloud;
  delete zmap;

  // destroy handles
  GoDestroy(system);
  GoDestroy(api);
}

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

  try
  {
    if (acquisition_mode == "Point Cloud")
      SetPointCloudMode();
    else
      SetZMapMode();


    if (GoSetup_SetExposure(setup, GO_ROLE_MAIN, exposure_time) != kOK)
    {
      throw(std::runtime_error("Failed to set Gocator sensor exposure"));
    }

    if (GoSetup_SetFrameRate(setup, frequency) != kOK)
    {
      throw(std::runtime_error("Failed to set Gocator sensor exposure"));
    }

    if (GoSetup_SetActiveAreaHeight(setup, GO_ROLE_MAIN, measurement_range) != kOK)
    {
      throw(std::runtime_error("Failed to set active area height"));
    }

    if (GoSetup_SetActiveAreaWidth(setup, GO_ROLE_MAIN, x_field_of_view) != kOK)
    {
      throw(std::runtime_error("Failed to set active area width"));
    }

    if (GoSetup_SetActiveAreaX(setup, GO_ROLE_MAIN, x_start) != kOK)
    {
      throw(std::runtime_error("Failed to set active area x origin"));
    }

    if (GoSetup_SetActiveAreaZ(setup, GO_ROLE_MAIN, z_start) != kOK)
    {
      throw(std::runtime_error("Failed to set active area z origin"));
    }

    if (GoSurfaceGenerationFixedLength_SetLength(surface, surface_length) != kOK)
    {
      throw(std::runtime_error("Failed to set surface fixed length"));
    }

    if (GoSensor_Flush(sensor) != kOK)
    {
      throw(std::runtime_error("Failed to initiate sensor configuration"));
    }

    // start Gocator sensor
    if (GoSystem_Start(system) != kOK)
    {
      throw(std::runtime_error("Failed to start Gocator sensor"));
    }

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

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

  try
  {
    if (GoSystem_ReceiveData(system, &dataset, timeout_ms) == kOK)
    {
      locked = true;

      // each result can have multiple data items
      // loop through all items in result message
      for (uint32_t i = 0; i < GoDataSet_Count(dataset); ++i)
      {
        GoDataMsg dataObj = GoDataSet_At(dataset, i);

        switch (GoDataMsg_Type(dataObj))
        {
        case GO_DATA_MESSAGE_TYPE_SURFACE_POINT_CLOUD:
        {
          GoSurfacePointCloudMsg surfacePointCloudMsg = dataObj;

          // get scale and offset to transform 16 bits data to real world metric positions
          float XResolution = static_cast<float>(NM_TO_MM(GoSurfacePointCloudMsg_XResolution(surfacePointCloudMsg)));
          float YResolution = static_cast<float>(NM_TO_MM(GoSurfacePointCloudMsg_YResolution(surfacePointCloudMsg)));
          float ZResolution = static_cast<float>(NM_TO_MM(GoSurfacePointCloudMsg_ZResolution(surfacePointCloudMsg)));
          float XOffset = static_cast<float>(UM_TO_MM(GoSurfacePointCloudMsg_XOffset(surfacePointCloudMsg)));
          float YOffset = static_cast<float>(UM_TO_MM(GoSurfacePointCloudMsg_YOffset(surfacePointCloudMsg)));
          float ZOffset = static_cast<float>(UM_TO_MM(GoSurfacePointCloudMsg_ZOffset(surfacePointCloudMsg)));

          uint32_t width = static_cast<uint32_t>(GoSurfacePointCloudMsg_Width(surfacePointCloudMsg));
          uint32_t length = static_cast<uint32_t>(GoSurfacePointCloudMsg_Length(surfacePointCloudMsg));

          std::vector<Easy3D::E3DPoint> pts;
          pts.reserve(width * length);

          Easy3D::E3DPoint p;
          for (uint32_t y = 0; y < length; y++)
          {
            kPoint3d16s *data = GoSurfacePointCloudMsg_RowAt(surfacePointCloudMsg, y);

            for (uint32_t x = 0; x < width; x++)
            {
              if (data[x].z != INVALID_RANGE_16BIT)
              {
                p.X = XOffset + XResolution * data[x].x;
                p.Y = YOffset + YResolution * data[x].y;
                p.Z = ZOffset + ZResolution * data[x].z;
                pts.push_back(p);
              }
            }
          }

          // Point cloud only store valid positions
          point_cloud = new Easy3D::EPointCloud();
          point_cloud->AddPoints(pts);
        }
        break;

        case GO_DATA_MESSAGE_TYPE_UNIFORM_SURFACE:
        {
          GoSurfaceMsg surfaceMsg = dataObj;

          uint32_t width = static_cast<uint32_t>(GoSurfaceMsg_Width(surfaceMsg));
          uint32_t length = static_cast<uint32_t>(GoSurfaceMsg_Length(surfaceMsg));

          // The resolution is the metric size of a pixel
          float XResolution = static_cast<float>(NM_TO_MM(GoSurfaceMsg_XResolution(surfaceMsg)));
          float YResolution = static_cast<float>(NM_TO_MM(GoSurfaceMsg_YResolution(surfaceMsg)));
          float ZResolution = static_cast<float>(NM_TO_MM(GoSurfaceMsg_ZResolution(surfaceMsg)));

          zmap = new Easy3D::EZMap16(width, length);
          zmap->SetResolution(XResolution, YResolution, ZResolution);
          uint16_t undef = zmap->GetUndefinedValue().Value;

          // Copy buffer to EZMap16
          for (uint32_t y = 0; y < length; ++y)
          {
            k16s *data = GoSurfaceMsg_RowAt(surfaceMsg, y);
            uint16_t *dst = (uint16_t*)zmap->GetCheckedBufferPtr(0, y);
            for (uint32_t x = 0; x < width; ++x, ++dst)
            {
              // use only valid data
              if (data[x] != INVALID_RANGE_16BIT)
              {
                *dst = data[x] - SHRT_MIN;  // convert from 16 bits signed to 16 bits unsigned
              }
              else
              {
                *dst = undef;
              }
            }
          }
        }
        break;
        default:;
        }
      }

      ++frame_counter;
    }
    else
    {
      throw(std::runtime_error("No data received during the waiting period"));
    }

    return frame_counter;
  }
  catch (std::exception& e)
  {
    ::MessageBox(NULL, CString(e.what()), _T("WaitFrame"), MB_OK | MB_ICONSTOP);
  }
  return 0;
}

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

  try
  {
    delete point_cloud;
    point_cloud = NULL;

    delete zmap;
    zmap = NULL;

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

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

  try
  {
    // stop Gocator sensor
    if (GoSystem_Stop(system) != kOK)
    {
      throw(std::runtime_error("Failed to stop Gocator sensor"));
    }

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

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

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

  map.Clear();
}

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

  // Push valid 3D points to an EPointCloud
  if (point_cloud != NULL)
  {
    dst_pc = *point_cloud;
  }
  else if (zmap != NULL)
  {
    dst_pc.Clear();
    Easy3D::EZMapToPointCloudConverter converter;
    converter.Convert(*zmap, dst_pc);
  }
  else
  {
    throw(std::runtime_error("Frame conversion failed"));
  }
}

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

  // LMI Gocator SDK cannot generate a ZMap
  // Use Easy3D::EPointCloudToZMapConverter instead

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


void LMIGrabber::InvokeParameterPanel()
{
  LMIParamDialog param_dialog(acquisition_mode, exposure_time, frequency, x_field_of_view, measurement_range, x_start, z_start, surface_length);

  if (param_dialog.DoModal() == IDOK)
  {
    acquisition_mode = param_dialog.acquisition_mode;
    exposure_time = param_dialog.exposure_time;
    frequency = param_dialog.frequency;
    x_field_of_view = param_dialog.x_field_of_view;
    measurement_range = param_dialog.measurement_range;
    x_start = param_dialog.x_start;
    z_start = param_dialog.z_start;
    surface_length = param_dialog.surface_length;
  }
}

void LMIGrabber::SetPointCloudMode()
{
  if (GoSetup_EnableUniformSpacing(setup, kFALSE) != kOK)
  {
    throw(std::runtime_error("Failed to enable uniform spacing mode"));
  }
  if (GoSetup_EnableIntensity(setup, kTRUE) != kOK)
  {
    throw(std::runtime_error("Failed to disable intensity mode"));
  }
}

void LMIGrabber::SetZMapMode()
{
  if (GoSetup_EnableUniformSpacing(setup, kTRUE) != kOK)
  {
    throw(std::runtime_error("Failed to enable uniform spacing mode"));
  }
  if (GoSetup_EnableIntensity(setup, kFALSE) != kOK)
  {
    throw(std::runtime_error("Failed to disable intensity mode"));
  }
}