/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers
//
#include <mrpt/hwdrivers/CImageGrabber_FlyCapture2.h>
#include <mrpt/system/datetime.h>
#include <mrpt/system/string_utils.h>

#if MRPT_HAS_FLYCAPTURE2
#include <FlyCapture2.h>
using namespace FlyCapture2;
#endif
#if MRPT_HAS_TRICLOPS
#include <fc2triclops.h>
#include <triclops.h>
using namespace Fc2Triclops;
#endif

#if MRPT_HAS_OPENCV
#include <opencv2/imgproc/imgproc_c.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#endif

#define CHECK_FC2_ERROR(_err)                                               \
  {                                                                         \
    if (_err != PGRERROR_OK)                                                \
    {                                                                       \
      THROW_EXCEPTION_FMT("FlyCapture2 error:\n%s", _err.GetDescription()); \
    }                                                                       \
  }
#define CHECK_TRICLOPS_ERROR(_err)                                               \
  {                                                                              \
    if (_err != TriclopsErrorOk)                                                 \
    {                                                                            \
      THROW_EXCEPTION_FMT("Triclops Error:\n'%s'", triclopsErrorToString(_err)); \
    }                                                                            \
  }
#define FC2_CAM      reinterpret_cast<FlyCapture2::Camera*>(m_camera)
#define FC2_CAM_INFO reinterpret_cast<FlyCapture2::CameraInfo*>(m_camera_info)
#define FC2_BUF_IMG  reinterpret_cast<FlyCapture2::Image*>(m_img_buffer)
#define TRI_CONTEXT  reinterpret_cast<TriclopsContext*>(m_triclops)

using namespace mrpt::hwdrivers;
using namespace std;

#if MRPT_HAS_FLYCAPTURE2
// Declare tables to convert strings to their #define values:
template <typename T>
struct fc2_str_val
{
  const char* str;
  T val;
};

const fc2_str_val<VideoMode> fc2_VideoMode_table[] = {
    {  "VIDEOMODE_160x120YUV444",   VIDEOMODE_160x120YUV444},
    {  "VIDEOMODE_320x240YUV422",   VIDEOMODE_320x240YUV422},
    {  "VIDEOMODE_640x480YUV411",   VIDEOMODE_640x480YUV411},
    {  "VIDEOMODE_640x480YUV422",   VIDEOMODE_640x480YUV422},
    {     "VIDEOMODE_640x480RGB",      VIDEOMODE_640x480RGB},
    {      "VIDEOMODE_640x480Y8",       VIDEOMODE_640x480Y8},
    {     "VIDEOMODE_640x480Y16",      VIDEOMODE_640x480Y16},
    {  "VIDEOMODE_800x600YUV422",   VIDEOMODE_800x600YUV422},
    {     "VIDEOMODE_800x600RGB",      VIDEOMODE_800x600RGB},
    {      "VIDEOMODE_800x600Y8",       VIDEOMODE_800x600Y8},
    {     "VIDEOMODE_800x600Y16",      VIDEOMODE_800x600Y16},
    { "VIDEOMODE_1024x768YUV422",  VIDEOMODE_1024x768YUV422},
    {    "VIDEOMODE_1024x768RGB",     VIDEOMODE_1024x768RGB},
    {     "VIDEOMODE_1024x768Y8",      VIDEOMODE_1024x768Y8},
    {    "VIDEOMODE_1024x768Y16",     VIDEOMODE_1024x768Y16},
    { "VIDEOMODE_1280x960YUV422",  VIDEOMODE_1280x960YUV422},
    {    "VIDEOMODE_1280x960RGB",     VIDEOMODE_1280x960RGB},
    {     "VIDEOMODE_1280x960Y8",      VIDEOMODE_1280x960Y8},
    {    "VIDEOMODE_1280x960Y16",     VIDEOMODE_1280x960Y16},
    {"VIDEOMODE_1600x1200YUV422", VIDEOMODE_1600x1200YUV422},
    {   "VIDEOMODE_1600x1200RGB",    VIDEOMODE_1600x1200RGB},
    {    "VIDEOMODE_1600x1200Y8",     VIDEOMODE_1600x1200Y8},
    {   "VIDEOMODE_1600x1200Y16",    VIDEOMODE_1600x1200Y16},
    {        "VIDEOMODE_FORMAT7",         VIDEOMODE_FORMAT7}
};
fc2_str_val<FrameRate> fc2_FrameRate_table[] = {
    {  "FRAMERATE_1_875",   FlyCapture2::FRAMERATE_1_875},
    {   "FRAMERATE_3_75",    FlyCapture2::FRAMERATE_3_75},
    {    "FRAMERATE_7_5",     FlyCapture2::FRAMERATE_7_5},
    {     "FRAMERATE_15",      FlyCapture2::FRAMERATE_15},
    {     "FRAMERATE_30",      FlyCapture2::FRAMERATE_30},
    {     "FRAMERATE_60",      FlyCapture2::FRAMERATE_60},
    {    "FRAMERATE_120",     FlyCapture2::FRAMERATE_120},
    {    "FRAMERATE_240",     FlyCapture2::FRAMERATE_240},
    {"FRAMERATE_FORMAT7", FlyCapture2::FRAMERATE_FORMAT7}
};
fc2_str_val<GrabMode> fc2_GrabMode_table[] = {
    {  "DROP_FRAMES",   DROP_FRAMES},
    {"BUFFER_FRAMES", BUFFER_FRAMES}
};

#define GET_CONV_TABLE(type)                                                       \
  vector<fc2_str_val<type>> fc2_vals_gen(type)                                     \
  {                                                                                \
    size_t n = sizeof(fc2_##type##_table) / sizeof(fc2_##type##_table[0]);         \
    vector<fc2_str_val<type>> vec(&fc2_##type##_table[0], &fc2_##type##_table[n]); \
    return vec;                                                                    \
  }
GET_CONV_TABLE(VideoMode)
GET_CONV_TABLE(FrameRate)
GET_CONV_TABLE(GrabMode)

template <typename T>
T fc2_defstr2num(const std::string& str)
{
  vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
  const std::string s = mrpt::system::trim(str);
  for (size_t i = 0; i < fc2_vals.size(); i++)
  {
    if (mrpt::system::strCmpI(fc2_vals[i].str, s.c_str())) return fc2_vals[i].val;
  }
  THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 constant: %s", s.c_str());
}

template <typename T>
const char* fc2_defnum2str(const T& val)
{
  vector<fc2_str_val<T>> fc2_vals = fc2_vals_gen(T());
  size_t i = static_cast<int>(val);
  if (i < fc2_vals.size())
    return fc2_vals[i].str;
  else
    THROW_EXCEPTION_FMT("Error: Unknown FlyCapture2 enum: %i", static_cast<int>(val));
}
#endif

TCaptureOptions_FlyCapture2::TCaptureOptions_FlyCapture2() :
    videomode(),  //("VIDEOMODE_640x480Y8"),
    framerate(),  // ("FRAMERATE_30"),
    grabmode("BUFFER_FRAMES")

{
  memset(camera_guid, 0, 4 * sizeof(camera_guid[0]));
}

void TCaptureOptions_FlyCapture2::loadOptionsFrom(
    const mrpt::config::CConfigFileBase& cfg, const std::string& sect, const std::string& prefix)
{
  camera_index = cfg.read_int(sect, prefix + string("camera_index"), camera_index);
  open_by_guid = cfg.read_bool(sect, prefix + string("open_by_guid"), open_by_guid);

  if (open_by_guid)
  {
    string sGUID = cfg.read_string(sect, prefix + string("camera_guid"), "", true);
    vector<string> sGUIDparts;
    mrpt::system::tokenize(sGUID, "- \t\r\n", sGUIDparts);
    ASSERTMSG_(
        sGUIDparts.size() == 4, "GUID format error: must have four blocks like XXX-XXX-XXX-XXX");

    for (int i = 0; i < 4; i++) sscanf(sGUIDparts[i].c_str(), "%X", &camera_guid[i]);
  }

  videomode = cfg.read_string(sect, prefix + string("videomode"), videomode);
  framerate = cfg.read_string(sect, prefix + string("framerate"), framerate);
  grabmode = cfg.read_string(sect, prefix + string("grabmode"), grabmode);
  numBuffers = cfg.read_uint64_t(sect, prefix + string("numBuffers"), numBuffers);
  grabTimeout = cfg.read_int(sect, prefix + string("grabTimeout"), grabTimeout);

  trigger_enabled = cfg.read_bool(sect, prefix + string("trigger_enabled"), trigger_enabled);
  trigger_polarity = cfg.read_int(sect, prefix + string("trigger_polarity"), trigger_polarity);
  trigger_source = cfg.read_int(sect, prefix + string("trigger_source"), trigger_source);
  trigger_mode = cfg.read_int(sect, prefix + string("trigger_mode"), trigger_mode);

  strobe_enabled = cfg.read_bool(sect, prefix + string("strobe_enabled"), strobe_enabled);
  strobe_source = cfg.read_int(sect, prefix + string("strobe_source"), strobe_source);
  strobe_polarity = cfg.read_int(sect, prefix + string("strobe_polarity"), strobe_polarity);
  strobe_delay = cfg.read_float(sect, prefix + string("strobe_delay"), strobe_delay);
  strobe_duration = cfg.read_float(sect, prefix + string("strobe_duration"), strobe_duration);

  autoexposure_auto = cfg.read_bool(sect, prefix + string("autoexposure_auto"), autoexposure_auto);
  autoexposure_onOff =
      cfg.read_bool(sect, prefix + string("autoexposure_onOFf"), autoexposure_onOff);
  autoexposure_abs = cfg.read_bool(sect, prefix + string("autoexposure_abs"), autoexposure_abs);
  autoexposure_EV = cfg.read_float(sect, prefix + string("autoexposure_EV"), autoexposure_EV);

  shutter_auto = cfg.read_bool(sect, prefix + string("shutter_auto"), shutter_auto);
  shutter_abs = cfg.read_bool(sect, prefix + string("shutter_abs"), shutter_abs);
  shutter_time_ms = cfg.read_float(sect, prefix + string("shutter_time_ms"), shutter_time_ms);

  gain_auto = cfg.read_bool(sect, prefix + string("gain_auto"), gain_auto);
  gain_abs = cfg.read_bool(sect, prefix + string("gain_abs"), gain_abs);
  gain_dB = cfg.read_float(sect, prefix + string("gain_dB"), gain_dB);

  stereo_mode = cfg.read_bool(sect, prefix + string("stereo_mode"), stereo_mode);
  rect_width = cfg.read_uint64_t(sect, prefix + string("rect_width"), rect_width);
  rect_height = cfg.read_uint64_t(sect, prefix + string("rect_height"), rect_height);
}

// ---------------------------------------------------------------
/** Default constructor */
CImageGrabber_FlyCapture2::CImageGrabber_FlyCapture2()
{
#if MRPT_HAS_FLYCAPTURE2
  m_img_buffer = new FlyCapture2::Image();
#endif
}

/** Constructor + open */
CImageGrabber_FlyCapture2::CImageGrabber_FlyCapture2(const TCaptureOptions_FlyCapture2& options) :
    m_camera(nullptr), m_camera_info(nullptr), m_img_buffer(nullptr), m_triclops(nullptr)
{
#if MRPT_HAS_FLYCAPTURE2
  m_img_buffer = new FlyCapture2::Image();
#endif
  this->open(options);
}

/** Destructor */
CImageGrabber_FlyCapture2::~CImageGrabber_FlyCapture2()
{
#if MRPT_HAS_FLYCAPTURE2
  this->close();
  delete FC2_BUF_IMG;
  m_img_buffer = nullptr;
#endif
}

/** Tries to open the camera with the given options. Raises an exception on
 * error. \sa close() */
void CImageGrabber_FlyCapture2::open(
    const TCaptureOptions_FlyCapture2& options, const bool startCapture)
{
#if MRPT_HAS_FLYCAPTURE2
  FlyCapture2::Error fe;

  cout << "[CImageGrabber_FlyCapture2::open] FlyCapture2 version: "
       << CImageGrabber_FlyCapture2::getFC2version() << std::endl;

  this->close();
  this->m_options = options;

  // Determine camera to open:
  // -----------------------------------
  PGRGuid guid;
  if (m_options.open_by_guid)
  {
    // Open by GUID:
    for (int i = 0; i < 4; i++) guid.value[i] = m_options.camera_guid[i];
  }
  else
  {
    // Open by camera index:
    BusManager busMgr;
    unsigned int numCameras;
    fe = busMgr.GetNumOfCameras(&numCameras);
    CHECK_FC2_ERROR(fe)

    if (m_options.camera_index >= numCameras)
      THROW_EXCEPTION(mrpt::format(
          "Error: camera_index to open is '%u', but only '%u' "
          "cameras were detected in the system.",
          m_options.camera_index, numCameras));

    fe = busMgr.GetCameraFromIndex(m_options.camera_index, &guid);
    CHECK_FC2_ERROR(fe)
  }

  // Connect to camera:
  m_camera = new FlyCapture2::Camera();
  m_camera_info = new FlyCapture2::CameraInfo();

  cout << mrpt::format(
      "[CImageGrabber_FlyCapture2::open] Opening camera with GUID= "
      "%08X-%08X-%08X-%08X...\n",
      guid.value[0], guid.value[1], guid.value[2], guid.value[3]);
  fe = FC2_CAM->Connect(&guid);
  CHECK_FC2_ERROR(fe)
  fe = FC2_CAM->GetCameraInfo(FC2_CAM_INFO);
  CHECK_FC2_ERROR(fe)

  const FlyCapture2::CameraInfo* ci = FC2_CAM_INFO;

  cout << mrpt::format(
      "[CImageGrabber_FlyCapture2::open] Camera connected ok:\n"
      " Serial number - %u\n"
      " Camera model - %s\n"
      " Camera vendor - %s\n"
      " Sensor - %s\n"
      " Resolution - %s\n"
      " Firmware version - %s\n"
      " Firmware build time - %s\n\n",
      ci->serialNumber, ci->modelName, ci->vendorName, ci->sensorInfo, ci->sensorResolution,
      ci->firmwareVersion, ci->firmwareBuildTime);

  // Set camera config:
  if (!m_options.videomode.empty() && !m_options.framerate.empty())
  {
    bool isSupported = false;

    if (!m_options.stereo_mode)
    {
      FlyCapture2::VideoMode vidMode = fc2_defstr2num<FlyCapture2::VideoMode>(m_options.videomode);
      FlyCapture2::FrameRate vidRate = fc2_defstr2num<FlyCapture2::FrameRate>(m_options.framerate);

      fe = FC2_CAM->GetVideoModeAndFrameRateInfo(vidMode, vidRate, &isSupported);
      CHECK_FC2_ERROR(fe)

      if (!isSupported)
      {
        FlyCapture2::VideoMode curVidMode;
        FlyCapture2::FrameRate curVidRate;
        fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);

        THROW_EXCEPTION(mrpt::format(
            "Camera mode '%s' + '%s' is not supported by this "
            "camera. Current mode is %d, current rate is %d.",
            m_options.videomode.c_str(), m_options.framerate.c_str(), static_cast<int>(curVidMode),
            static_cast<int>(curVidRate)));
      }

      fe = FC2_CAM->SetVideoModeAndFrameRate(vidMode, vidRate);
      CHECK_FC2_ERROR(fe)
    }
    else
    {
#if MRPT_HAS_TRICLOPS
      Fc2Triclops::ErrorType fte;
      // Configure camera for Stereo mode
      StereoCameraMode mode = TWO_CAMERA;
      fte = setStereoMode(*(FC2_CAM), mode);
      if (fte) handleFc2TriclopsError(fte, "setStereoMode");

      // Generate Triclops context

      m_triclops = new TriclopsContext;
      fte = getContextFromCamera(FC2_CAM_INFO->serialNumber, TRI_CONTEXT);
      if (fte != ERRORTYPE_OK) handleFc2TriclopsError(fte, "getContextFromCamera");

      //  ------------------------------------------------------
      //   TRICLOPS CONFIGURATION
      //  ------------------------------------------------------
      // Current Format7 settings
      /*
      Format7ImageSettings f7settings;
      unsigned int f7PacketSize;
      float f7Percentage;
      fe = FC2_CAM->GetFormat7Configuration(&f7settings, &f7PacketSize,
      &f7Percentage);
      CHECK_FC2_ERROR(fe)
      */

      TriclopsError te;
      // Set rectified resolution
      te = triclopsSetResolution(*(TRI_CONTEXT), m_options.rect_height, m_options.rect_width);
      CHECK_TRICLOPS_ERROR(te);
      // Retrieve camera parameters
      te = triclopsGetBaseline(*(TRI_CONTEXT), &m_baseline);
      CHECK_TRICLOPS_ERROR(te);
      te = triclopsGetFocalLength(*(TRI_CONTEXT), &m_focalLength);
      CHECK_TRICLOPS_ERROR(te);
      te = triclopsGetImageCenter(*(TRI_CONTEXT), &m_centerRow, &m_centerCol);
      CHECK_TRICLOPS_ERROR(te);
#else
      THROW_EXCEPTION("MRPT compiled without support for Triclops");
#endif
    }
  }

  {
    FlyCapture2::VideoMode curVidMode;
    FlyCapture2::FrameRate curVidRate;
    fe = FC2_CAM->GetVideoModeAndFrameRate(&curVidMode, &curVidRate);
    if (fe == PGRERROR_OK)
      cout << mrpt::format(
          "[CImageGrabber_FlyCapture2::open] Current camera mode is %s, "
          "current rate is %s.\n",
          fc2_defnum2str<FlyCapture2::VideoMode>(curVidMode),
          fc2_defnum2str<FlyCapture2::FrameRate>(curVidRate));
  }

  // Set trigger:
  FlyCapture2::TriggerModeInfo trigInfo;
  FC2_CAM->GetTriggerModeInfo(&trigInfo);

  FlyCapture2::TriggerMode trig;
  trig.onOff = m_options.trigger_enabled;
  if (m_options.trigger_enabled)
  {
    trig.mode = m_options.trigger_mode;
    trig.polarity = m_options.trigger_polarity;
    trig.source = m_options.trigger_source;
  }
  fe = FC2_CAM->SetTriggerMode(&trig);
  CHECK_FC2_ERROR(fe)

  // Strobe:
  if (m_options.strobe_enabled)
  {
    FlyCapture2::StrobeControl strobe;

    strobe.onOff = m_options.strobe_enabled;
    strobe.delay = m_options.strobe_delay;
    strobe.duration = m_options.strobe_duration;
    strobe.polarity = m_options.strobe_polarity;
    strobe.source = m_options.strobe_source;

    fe = FC2_CAM->SetStrobe(&strobe);
    CHECK_FC2_ERROR(fe)
  }

  // Set configs:
  FlyCapture2::FC2Config fc2conf;
  FC2_CAM->GetConfiguration(&fc2conf);
  CHECK_FC2_ERROR(fe)

  fc2conf.grabMode = fc2_defstr2num<FlyCapture2::GrabMode>(m_options.grabmode);
  if (m_options.grabTimeout >= 0) fc2conf.grabTimeout = m_options.grabTimeout;

  fc2conf.numBuffers = m_options.numBuffers;

  fe = FC2_CAM->SetConfiguration(&fc2conf);
  CHECK_FC2_ERROR(fe)

  // Autoexposure:
  {
    FlyCapture2::Property p;
    p.type = FlyCapture2::AUTO_EXPOSURE;
    FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
    CHECK_FC2_ERROR(error)
    p.autoManualMode = m_options.autoexposure_auto;  // true=auto
    p.onOff = m_options.autoexposure_onOff;          // true=on
    p.absControl = m_options.autoexposure_abs;       // true=abs
    p.absValue = m_options.autoexposure_EV;          // abs value in Exposure Value (EV)
    fe = FC2_CAM->SetProperty(&p);
    CHECK_FC2_ERROR(fe)
  }

  // Brightness:
  {
    FlyCapture2::Property p;
    p.type = FlyCapture2::BRIGHTNESS;
    FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
    CHECK_FC2_ERROR(error)
    p.autoManualMode = true;  // true=auto
    // p.absControl = true;
    // p.absValue = Brightness;
    fe = FC2_CAM->SetProperty(&p);
    CHECK_FC2_ERROR(fe)
  }

  // Shutter:
  {
    FlyCapture2::Property p;
    p.type = FlyCapture2::SHUTTER;
    FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
    CHECK_FC2_ERROR(error)
    p.autoManualMode = m_options.shutter_auto;  // true=auto
    p.absControl = m_options.shutter_abs;       // true=abs
    p.absValue = m_options.shutter_time_ms;
    // p.onOff = false;
    fe = FC2_CAM->SetProperty(&p);
    CHECK_FC2_ERROR(fe)
  }

  // Gain:
  {
    FlyCapture2::Property p;
    p.type = FlyCapture2::GAIN;
    FlyCapture2::Error error = FC2_CAM->GetProperty(&p);
    CHECK_FC2_ERROR(error)
    p.autoManualMode = m_options.gain_auto;  // true=auto
    p.absControl = m_options.gain_abs;       // true=abs
    p.absValue = m_options.gain_dB;          // abs value in dB (decibeles)
    // p.onOff = false;
    fe = FC2_CAM->SetProperty(&p);
    CHECK_FC2_ERROR(fe)
  }

  // Framecounter:
  EmbeddedImageInfo eii;
  fe = FC2_CAM->GetEmbeddedImageInfo(&eii);
  if (fe == PGRERROR_OK)
  {
    if (eii.frameCounter.available) eii.frameCounter.onOff = true;
    if (eii.timestamp.available) eii.timestamp.onOff = true;
    if (eii.exposure.available) eii.exposure.onOff = true;
    if (eii.brightness.available) eii.brightness.onOff = true;

    // Enable all:
    FC2_CAM->SetEmbeddedImageInfo(&eii);
  }

  // Start:
  if (startCapture) this->startCapture();
#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/** Start the actual image capture of the camera. Must be called after open(),
 * only when "startCapture" was set to false. */
void CImageGrabber_FlyCapture2::startCapture()
{
#if MRPT_HAS_FLYCAPTURE2
  if (!m_camera)
  {
    THROW_EXCEPTION("Camera is not opened. Call open() first.");
  }

  FlyCapture2::Error error = FC2_CAM->StartCapture();
  CHECK_FC2_ERROR(error)

#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/** Starts a synchronous capture of several cameras, which must have been
 * already opened. */
void CImageGrabber_FlyCapture2::startSyncCapture(
    int numCameras, const CImageGrabber_FlyCapture2** cameras_array)
{
#if MRPT_HAS_FLYCAPTURE2

  std::vector<const FlyCapture2::Camera*> cam_ptrs(numCameras);

  for (int i = 0; i < numCameras; i++)
  {
    const CImageGrabber_FlyCapture2* obj = cameras_array[i];
    if (!obj->m_camera)
    {
      THROW_EXCEPTION_FMT("Camera #%i in list is not opened. Call open() first.", i);
    }

    FlyCapture2::Camera* cam = reinterpret_cast<FlyCapture2::Camera*>(obj->m_camera);
    cam_ptrs[i] = cam;
  }

  if (!cam_ptrs.empty())
  {
    FlyCapture2::Error error = FlyCapture2::Camera::StartSyncCapture(cam_ptrs.size(), &cam_ptrs[0]);
    CHECK_FC2_ERROR(error)
  }
#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/** Stop capture. */
void CImageGrabber_FlyCapture2::stopCapture()
{
#if MRPT_HAS_FLYCAPTURE2
  if (m_camera)
  {
    Error error;

    // Stop grabbing:
    error = FC2_CAM->StopCapture();
    CHECK_FC2_ERROR(error)
  }
#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/** Closes the opened camera, if any. Called automatically on object
 * destruction. */
void CImageGrabber_FlyCapture2::close()
{
#if MRPT_HAS_FLYCAPTURE2
  try
  {
    this->stopCapture();
  }
  catch (...)
  {
  }

  // Disconnect the camera
  try
  {
    if (m_camera) FC2_CAM->Disconnect();
  }
  catch (...)
  {
  }

  // Delete objects:
  try
  {
    if (m_camera) delete FC2_CAM;
  }
  catch (...)
  {
  }
  try
  {
    if (m_camera_info) delete FC2_CAM_INFO;
  }
  catch (...)
  {
  }
#if MRPT_HAS_TRICLOPS
  try
  {
    if (m_triclops) delete TRI_CONTEXT;
  }
  catch (...)
  {
  }
#endif

  m_camera = nullptr;
  m_camera_info = nullptr;

#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/** Returns the PGR FlyCapture2 library version */
std::string CImageGrabber_FlyCapture2::getFC2version()
{
#if MRPT_HAS_FLYCAPTURE2
  FlyCapture2::FC2Version fc2Version;
  FlyCapture2::Utilities::GetLibraryVersion(&fc2Version);
  return mrpt::format(
      "%d.%d.%d.%d", fc2Version.major, fc2Version.minor, fc2Version.type, fc2Version.build);
#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/*-------------------------------------------------------------
          get the image - MONO
 -------------------------------------------------------------*/
// Grab image from the camera. This method blocks until the next frame is
// captured.
// return: false on any error.
bool CImageGrabber_FlyCapture2::getObservation(mrpt::obs::CObservationImage& out_observation)
{
#if MRPT_HAS_FLYCAPTURE2
  if (!m_camera)
  {
    std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
                 "not opened. Call open() first.\n";
    return false;
  }
  try
  {
    FlyCapture2::Error error;
    FlyCapture2::Image image;
    error = FC2_CAM->RetrieveBuffer(&image);
    CHECK_FC2_ERROR(error)

    FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();
    // White balance, etc.
    // FlyCapture2::ImageMetadata imd = image.GetMetadata();
    // Determine if it's B/W or color:
    FlyCapture2::PixelFormat pf = image.GetPixelFormat();
    const bool is_color =
        pf == PIXEL_FORMAT_RGB8 || pf == PIXEL_FORMAT_RGB16 || pf == PIXEL_FORMAT_S_RGB16 ||
        pf == PIXEL_FORMAT_RAW8 || pf == PIXEL_FORMAT_RAW16 || pf == PIXEL_FORMAT_RAW12 ||
        pf == PIXEL_FORMAT_BGR || pf == PIXEL_FORMAT_BGRU || pf == PIXEL_FORMAT_RGBU ||
        pf == PIXEL_FORMAT_BGR16 || pf == PIXEL_FORMAT_BGRU16 || pf == PIXEL_FORMAT_422YUV8_JPEG;
    // Decode image:
    error = image.Convert(is_color ? PIXEL_FORMAT_BGR : PIXEL_FORMAT_MONO8, FC2_BUF_IMG);
    CHECK_FC2_ERROR(error)
    // Convert PGR FlyCapture2 image ==> OpenCV format:
    unsigned int img_rows, img_cols, img_stride;
    FC2_BUF_IMG->GetDimensions(&img_rows, &img_cols, &img_stride);
    out_observation.image.loadFromMemoryBuffer(
        img_cols, img_rows, is_color, FC2_BUF_IMG->GetData());
    // It seems timestamp is not always correctly filled in the incoming
    // imgs:
    if (timestamp.seconds != 0)
      out_observation.timestamp =
          mrpt::Clock::fromDouble(timestamp.seconds + 1e-6 * timestamp.microSeconds);
    else
      out_observation.timestamp = mrpt::Clock::now();
    return true;
  }
  catch (const std::exception& e)
  {
    std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n" << e.what() << std::endl;
    return false;
  }
#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2");
#endif
}

/*-------------------------------------------------------------
          get the image - STEREO
 -------------------------------------------------------------*/
// Grab image from the camera. This method blocks until the next frame is
// captured.
// return: false on any error.
bool CImageGrabber_FlyCapture2::getObservation(mrpt::obs::CObservationStereoImages& out_observation)
{
#if MRPT_HAS_FLYCAPTURE2 && MRPT_HAS_TRICLOPS && MRPT_HAS_OPENCV
  if (!m_camera)
  {
    std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Camera is "
                 "not opened. Call open() first.\n";
    return false;
  }

  try
  {
    FlyCapture2::Error ferr;
    Fc2Triclops::ErrorType fterr;
    FlyCapture2::Image image;
    ferr = FC2_CAM->RetrieveBuffer(&image);
    CHECK_FC2_ERROR(ferr)
    mrpt::system::TTimeStamp ts_retrieved = mrpt::Clock::now();
    FlyCapture2::TimeStamp timestamp = image.GetTimeStamp();

    // White balance, etc.
    // FlyCapture2::ImageMetadata imd = image.GetMetadata();

    // ------------------------------------------
    // Extract images from common interleaved image:
    // ------------------------------------------
    FlyCapture2::Image rawImage[2];

    // Convert the pixel interleaved raw data to de-interleaved raw data
    fterr = Fc2Triclops::unpackUnprocessedRawOrMono16Image(
        image, true,
        rawImage[0],   // Right image
        rawImage[1]);  // Left image
    if (fterr != Fc2Triclops::ERRORTYPE_OK)
    {
      Fc2Triclops::handleFc2TriclopsError(fterr, "unprocessedRawOrMono16Image()");
      return false;
    }

    // Convert each raw image to RGBU image (for color images)
    unsigned int img_rows, img_cols, img_stride;
    for (int i = 0; i < 2; ++i)
    {
      FlyCapture2::Image rgbuImage;
      ferr = rawImage[i].SetColorProcessing(FlyCapture2::HQ_LINEAR);
      CHECK_FC2_ERROR(ferr)
      ferr = rawImage[i].Convert(PIXEL_FORMAT_BGR, &rgbuImage);
      CHECK_FC2_ERROR(ferr)

      // get unrectified images:
      rgbuImage.GetDimensions(&img_rows, &img_cols, &img_stride);
      uint8_t* ptrData = rgbuImage.GetData();

      // Convert PGR image ==> OpenCV format:
      auto tmpImg = cv::Mat(img_rows, img_cols, CV_8UC3, ptrData, img_stride);

      // Fill output stereo observation
      auto& im = (i == 0 ? out_observation.imageRight : out_observation.imageLeft);
      im = mrpt::img::CImage(tmpImg, mrpt::img::DEEP_COPY);
    }

    out_observation.hasImageRight = true;

    // It seems timestamp is not always correctly filled in the incoming
    // imgs:
    if (timestamp.seconds != 0)
      out_observation.timestamp =
          mrpt::Clock::fromDouble(timestamp.seconds + 1e-6 * timestamp.microSeconds);
    else
      out_observation.timestamp = ts_retrieved;

    out_observation.rightCameraPose.x(m_baseline);
    out_observation.rightCameraPose.y(0);
    out_observation.rightCameraPose.z(0);

    out_observation.rightCameraPose.quat().r(1);
    out_observation.rightCameraPose.quat().x(0);
    out_observation.rightCameraPose.quat().y(0);
    out_observation.rightCameraPose.quat().z(0);

    out_observation.cameraPose.x(0);
    out_observation.cameraPose.y(0);
    out_observation.cameraPose.z(0);

    out_observation.cameraPose.quat().r(1);
    out_observation.cameraPose.quat().x(0);
    out_observation.cameraPose.quat().y(0);
    out_observation.cameraPose.quat().z(0);

    out_observation.leftCamera.setIntrinsicParamsFromValues(
        m_focalLength, m_focalLength, m_centerCol, m_centerRow);
    out_observation.rightCamera.setIntrinsicParamsFromValues(
        m_focalLength, m_focalLength, m_centerCol, m_centerRow);
    return true;
  }
  catch (const std::exception& e)
  {
    std::cerr << "[CImageGrabber_FlyCapture2::getObservation] Error:\n" << e.what() << std::endl;
    return false;
  }
#else
  THROW_EXCEPTION("MRPT compiled without support for FlyCapture2, Triclops or OpenCV");
#endif
}
