vp1394TwoGrabber.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2025 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See https://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Firewire cameras video capture.
 */

#include <iostream>
 
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
 
/*
 * Interface with libdc1394 2.x
 */
#if defined(VISP_HAVE_DC1394)
#include <unistd.h>
 
#include <visp3/core/vpFrameGrabberException.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpTime.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
 
BEGIN_VISP_NAMESPACE
const char *vp1394TwoGrabber::strVideoMode[DC1394_VIDEO_MODE_NUM] = {
    "MODE_160x120_YUV444",  "MODE_320x240_YUV422",  "MODE_640x480_YUV411",   "MODE_640x480_YUV422",
    "MODE_640x480_RGB8",    "MODE_640x480_MONO8",   "MODE_640x480_MONO16",   "MODE_800x600_YUV422",
    "MODE_800x600_RGB8",    "MODE_800x600_MONO8",   "MODE_1024x768_YUV422",  "MODE_1024x768_RGB8",
    "MODE_1024x768_MONO8",  "MODE_800x600_MONO16",  "MODE_1024x768_MONO16",  "MODE_1280x960_YUV422",
    "MODE_1280x960_RGB8",   "MODE_1280x960_MONO8",  "MODE_1600x1200_YUV422", "MODE_1600x1200_RGB8",
    "MODE_1600x1200_MONO8", "MODE_1280x960_MONO16", "MODE_1600x1200_MONO16", "MODE_EXIF",
    "MODE_FORMAT7_0",       "MODE_FORMAT7_1",       "MODE_FORMAT7_2",        "MODE_FORMAT7_3",
    "MODE_FORMAT7_4",       "MODE_FORMAT7_5",       "MODE_FORMAT7_6",        "MODE_FORMAT7_7" };
 
const char *vp1394TwoGrabber::strFramerate[DC1394_FRAMERATE_NUM] = {
    "FRAMERATE_1_875", "FRAMERATE_3_75", "FRAMERATE_7_5", "FRAMERATE_15",
    "FRAMERATE_30",    "FRAMERATE_60",   "FRAMERATE_120", "FRAMERATE_240" };
 
const char *vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM] = {
    "COLOR_CODING_MONO8",  "COLOR_CODING_YUV411", "COLOR_CODING_YUV422", "COLOR_CODING_YUV444",
    "COLOR_CODING_RGB8",   "COLOR_CODING_MONO16", "COLOR_CODING_RGB16",  "COLOR_CODING_MONO16S",
    "COLOR_CODING_RGB16S", "COLOR_CODING_RAW8",   "COLOR_CODING_RAW16",
};

vp1394TwoGrabber::vp1394TwoGrabber(bool reset)
  : camera(nullptr), cameras(nullptr), num_cameras(0), camera_id(0), verbose(false), camIsOpen(nullptr),
  num_buffers(4), // ring buffer size
  isDataModified(nullptr), initialShutterMode(nullptr), dataCam(nullptr)
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  ,
  d(nullptr), list(nullptr)
#endif
{
  // protected members
  width = height = 0;
 
  // private members
  init = false;
 
  reset = false;
  initialize(reset);
 
  //  open();
}

vp1394TwoGrabber::~vp1394TwoGrabber()
{
  /*  if(num_cameras >= 1){
      delete[] isDataModified;
      delete[] initialShutterMode;
      delete[] dataCam;
    }*/
  close();
}

void vp1394TwoGrabber::setCamera(uint64_t cam_id)
{
  // Suppose that if camera_id is a camera GUID, this value is greater
  // than the number of cameras connected to the bus
  if (cam_id >= num_cameras) {
    // Check if camera_id is a camera guid
    bool is_guid = false;
    // check if the camera_id is a guid
    for (unsigned int i = 0; i < num_cameras; i++) {
      if (cameras[i]->guid == cam_id) {
        this->camera_id = i;
        is_guid = true;
        break;
      }
    }
    if (is_guid == false) {
      std::cout << "Error: The camera with guid 0x" << std::hex << cam_id << " is not present" << std::endl;
      std::cout << num_cameras << " camera(s) connected" << std::endl;
      for (unsigned int i = 0; i < num_cameras; i++) {
        std::cout << " - camera " << i << " with guid 0x" << std::hex << cameras[i]->guid << std::endl;
      }
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required camera is not present"));
    }
  }
  else {
    this->camera_id = static_cast<unsigned int>(cam_id);  // The input cam_id is not a
                                                          // uint64_t guid, but the index of
                                                          // the camera
  }
 
  // create a pointer to the working camera
  camera = cameras[this->camera_id];
}

void vp1394TwoGrabber::getCamera(uint64_t &cam_id)
{
  if (num_cameras) {
    cam_id = this->camera_id;
  }
  else {
    close();
    vpERROR_TRACE("No cameras found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found"));
  }
}

uint64_t vp1394TwoGrabber::getCamera()
{
  if (num_cameras) {
    return this->camera_id;
  }
  else {
    close();
    vpERROR_TRACE("No cameras found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found"));
  }
}

void vp1394TwoGrabber::getNumCameras(unsigned int &ncameras) const
{
  if (!num_cameras) {
    vpCTRACE << "No camera found..." << std::endl;
    ncameras = 0;
  }
 
  ncameras = num_cameras;
}

unsigned int vp1394TwoGrabber::getNumCameras() const
{
  unsigned int ncameras = 0;
  if (!num_cameras) {
    vpCTRACE << "No camera found..." << std::endl;
    ncameras = 0;
  }
 
  ncameras = num_cameras;
  return ncameras;
}

void vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode)
{
  open();
  if (!num_cameras) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  if (!isVideoModeSupported(videomode)) {
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Video mode not supported by camera %d",
                                  camera_id));
  }
  // Stop dma capture if started
  setTransmission(DC1394_OFF);
  setCapture(DC1394_OFF);
 
  if (dc1394_video_set_mode(camera, (dc1394video_mode_t)videomode) != DC1394_SUCCESS) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video mode"));
  }
 
  setCapture(DC1394_ON);
  setTransmission(DC1394_ON);
 
  // Updates image size from new video mode
  if (dc1394_get_image_size_from_video_mode(camera, (dc1394video_mode_t)videomode, &this->width, &this->height) !=
      DC1394_SUCCESS) {
 
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get image size"));
  }
}

void vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType &videomode)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394video_mode_t _videomode;
  if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
 
    close();
    vpERROR_TRACE("Can't get current video mode");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode"));
  }
  videomode = (vp1394TwoVideoModeType)_videomode;
}

uint32_t vp1394TwoGrabber::getVideoModeSupported(std::list<vp1394TwoVideoModeType> &videomodes)
{
  // Refresh the list of supported modes
  videomodes.clear();
 
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  dc1394video_modes_t _videomodes;
 
  // get video modes:
  if (dc1394_video_get_supported_modes(camera, &_videomodes) != DC1394_SUCCESS) {
 
    close();
    vpERROR_TRACE("Can't get video modes");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get video modes"));
  }
 
  // parse the video modes to add in the list
  for (unsigned i = 0; i < _videomodes.num; i++) {
    vp1394TwoVideoModeType _mode = (vp1394TwoVideoModeType)_videomodes.modes[i];
    videomodes.push_back(_mode);
  }
 
  // return the number of available video modes
  return _videomodes.num;
}

bool vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  dc1394video_modes_t _videomodes;
 
  // get video modes:
  if (dc1394_video_get_supported_modes(camera, &_videomodes) != DC1394_SUCCESS) {
 
    close();
    vpERROR_TRACE("Can't get video modes");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get video modes"));
  }
 
  // parse the video modes to check with the desired
  for (unsigned i = 0; i < _videomodes.num; i++) {
    if ((vp1394TwoVideoModeType)_videomodes.modes[i] == videomode) {
      return true;
    }
  }
  return false;
}

bool vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
{
 
  if (dc1394_is_video_mode_scalable((dc1394video_mode_t)videomode))
    return true;
 
  return false;
}

bool vp1394TwoGrabber::isColor()
{
  vp1394TwoColorCodingType coding;
  getColorCoding(coding);
 
  switch (coding) {
  case vpCOLOR_CODING_MONO8:
  case vpCOLOR_CODING_MONO16:
  case vpCOLOR_CODING_MONO16S:
  case vpCOLOR_CODING_RAW8:
  case vpCOLOR_CODING_RAW16:
    return false;
  case vpCOLOR_CODING_YUV411:
  case vpCOLOR_CODING_YUV422:
  case vpCOLOR_CODING_YUV444:
  case vpCOLOR_CODING_RGB8:
  case vpCOLOR_CODING_RGB16:
  case vpCOLOR_CODING_RGB16S:
    return true;
  }
  return false;
}

void vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
{
  open();
  if (!num_cameras) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  vp1394TwoVideoModeType cur_videomode;
  getVideoMode(cur_videomode);
  if (isVideoModeFormat7(cur_videomode))
    return;
 
  if (!isFramerateSupported(cur_videomode, fps)) {
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Framerate not supported by camera %d",
                                  camera_id));
  }
 
  // Stop dma capture if started
  setTransmission(DC1394_OFF);
  setCapture(DC1394_OFF);
 
  if (dc1394_video_set_framerate(camera, (dc1394framerate_t)fps) != DC1394_SUCCESS) {
 
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set framerate"));
  }
 
  setCapture(DC1394_ON);
  setTransmission(DC1394_ON);
}

void vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType &fps)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  dc1394framerate_t _fps;
  if (dc1394_video_get_framerate(camera, &_fps) != DC1394_SUCCESS) {
 
    close();
    vpERROR_TRACE("Can't get current framerate");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current framerate"));
  }
  fps = (vp1394TwoFramerateType)_fps;
}

uint32_t vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode, std::list<vp1394TwoFramerateType> &fps)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  // Refresh the list of supported framerates
  fps.clear();
 
  switch (mode) {
  // Framerate not available for:
  //  - vpVIDEO_MODE_EXIF ie Format_6
  //  - vpVIDEO_MODE_FORMAT7... ie the Format_7
  case vpVIDEO_MODE_EXIF:
  case vpVIDEO_MODE_FORMAT7_0:
  case vpVIDEO_MODE_FORMAT7_1:
  case vpVIDEO_MODE_FORMAT7_2:
  case vpVIDEO_MODE_FORMAT7_3:
  case vpVIDEO_MODE_FORMAT7_4:
  case vpVIDEO_MODE_FORMAT7_5:
  case vpVIDEO_MODE_FORMAT7_6:
  case vpVIDEO_MODE_FORMAT7_7:
    return 0;
    break;
  default: {
    dc1394framerates_t _fps;
    if (dc1394_video_get_supported_framerates(camera, (dc1394video_mode_t)mode, &_fps) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported frametates for mode %d\n", mode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported framerates"));
    }
    if (_fps.num == 0)
      return 0;
 
    for (unsigned int i = 0; i < _fps.num; i++)
      fps.push_back((vp1394TwoFramerateType)_fps.framerates[i]);
 
    return _fps.num;
  } break;
  }
}

bool vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode, vp1394TwoFramerateType fps)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  switch (mode) {
  // Framerate not available for:
  //  - vpVIDEO_MODE_EXIF ie Format_6
  //  - vpVIDEO_MODE_FORMAT7... ie the Format_7
  case vpVIDEO_MODE_EXIF:
  case vpVIDEO_MODE_FORMAT7_0:
  case vpVIDEO_MODE_FORMAT7_1:
  case vpVIDEO_MODE_FORMAT7_2:
  case vpVIDEO_MODE_FORMAT7_3:
  case vpVIDEO_MODE_FORMAT7_4:
  case vpVIDEO_MODE_FORMAT7_5:
  case vpVIDEO_MODE_FORMAT7_6:
  case vpVIDEO_MODE_FORMAT7_7:
    return 0;
    break;
  default: {
    dc1394framerates_t _fps;
    if (dc1394_video_get_supported_framerates(camera, (dc1394video_mode_t)mode, &_fps) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported frametates for mode %d\n", mode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported framerates"));
    }
    if (_fps.num == 0)
      return 0;
 
    for (unsigned int i = 0; i < _fps.num; i++) {
      if (fps == (vp1394TwoFramerateType)_fps.framerates[i]) {
        return true;
      }
    }
    return false;
  } break;
  }
}

void vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding)
{
  if (!num_cameras) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394video_mode_t _videomode;
  if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
 
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode"));
  }
 
  if (!isColorCodingSupported((vp1394TwoVideoModeType)_videomode, coding)) {
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Color coding not supported by camera %d",
                                  camera_id));
  }
 
  // Format 7 video mode
  if (dc1394_is_video_mode_scalable(_videomode)) {
    setTransmission(DC1394_OFF);
    setCapture(DC1394_OFF);
 
    if (dc1394_format7_set_color_coding(camera, _videomode, (dc1394color_coding_t)coding) != DC1394_SUCCESS) {
 
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set color coding"));
    }
 
    setCapture(DC1394_ON);
    setTransmission(DC1394_ON);
  }
}

void vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType &coding)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  dc1394video_mode_t _videomode;
  if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
 
    close();
    vpERROR_TRACE("Can't get current video mode");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode"));
  }
 
  dc1394color_coding_t _coding;
  if (dc1394_is_video_mode_scalable(_videomode)) {
    // Format 7 video mode
    if (dc1394_format7_get_color_coding(camera, _videomode, &_coding) != DC1394_SUCCESS) {
 
      close();
      vpERROR_TRACE("Can't get current color coding");
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't query current color coding"));
    }
  }
  else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)_videomode)) {
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "No color coding for format 6 video mode"));
  }
  else {
 // Not Format 7 and not Format 6 video modes
    if (dc1394_get_color_coding_from_video_mode(camera, (dc1394video_mode_t)_videomode, &_coding) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported color coding for mode %d\n", _videomode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't query current color coding"));
    }
  }
  coding = (vp1394TwoColorCodingType)_coding;
}

uint32_t vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
                                                   std::list<vp1394TwoColorCodingType> &codings)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  // Refresh the list of supported framerates
  codings.clear();
 
  if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
    // Format 7 video mode
    dc1394color_codings_t _codings;
    if (dc1394_format7_get_color_codings(camera, (dc1394video_mode_t)mode, &_codings) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported color codings for mode %d\n", mode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported color codings"));
    }
    if (_codings.num == 0)
      return 0;
 
    for (unsigned int i = 0; i < _codings.num; i++)
      codings.push_back((vp1394TwoColorCodingType)_codings.codings[i]);
 
    return _codings.num;
  }
  else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
 // Format 6 video mode
    return 0;
  }
  else {
 // Not Format 7 and not Format 6 video modes
    dc1394color_coding_t _coding;
    if (dc1394_get_color_coding_from_video_mode(camera, (dc1394video_mode_t)mode, &_coding) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported color coding for mode %d\n", mode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported color coding"));
    }
    codings.push_back((vp1394TwoColorCodingType)_coding);
    return 1;
  }
}

bool vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode, vp1394TwoColorCodingType coding)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  if (dc1394_is_video_mode_scalable((dc1394video_mode_t)mode)) {
    // Format 7 video mode
    dc1394color_codings_t _codings;
    if (dc1394_format7_get_color_codings(camera, (dc1394video_mode_t)mode, &_codings) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported color codings for mode %d\n", mode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported color codings"));
    }
    if (_codings.num == 0)
      return 0;
 
    for (unsigned int i = 0; i < _codings.num; i++) {
      if (coding == (vp1394TwoColorCodingType)_codings.codings[i])
        return true;
    }
    return false;
  }
  else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
 // Format 6 video mode
    return false;
  }
  else {
 // Not Format 7 and not Format 6 video modes
    dc1394color_coding_t _coding;
    if (dc1394_get_color_coding_from_video_mode(camera, (dc1394video_mode_t)mode, &_coding) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Could not query supported color coding for mode %d\n", mode);
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported color coding"));
      return false;
    }
    if (coding == (vp1394TwoColorCodingType)_coding)
      return true;
 
    return false;
  }
}

void vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top, unsigned int w, unsigned int h)
{
  open();
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394video_mode_t _videomode;
  if (dc1394_video_get_mode(camera, &_videomode) != DC1394_SUCCESS) {
 
    close();
    vpERROR_TRACE("Can't get current video mode");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode"));
  }
  if (dc1394_is_video_mode_scalable(_videomode)) {
    // Stop dma capture if started
    setTransmission(DC1394_OFF);
    setCapture(DC1394_OFF);
    // Format 7 video mode
    unsigned int max_width, max_height;
    if (dc1394_format7_get_max_image_size(camera, _videomode, &max_width, &max_height) != DC1394_SUCCESS) {
 
      close();
      vpERROR_TRACE("Can't get format7 max image size");
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get format7 max image size"));
    }
#if 0
    vpTRACE("left: %d top: %d width: %d height: %d", left, top,
            width == 0 ? DC1394_USE_MAX_AVAIL : w,
            height == 0 ? DC1394_USE_MAX_AVAIL : h);
    vpTRACE("max_width: %d max_height: %d", max_width, max_height);
#endif
 
    if (left > max_width) {
      vpERROR_TRACE("Can't set format7 ROI");
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set format7 ROI"));
    }
    if (top > max_height) {
      vpERROR_TRACE("Can't set format7 ROI");
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set format7 ROI"));
    }
 
    int32_t roi_width;
    int32_t roi_height;
 
    if (w != 0) {
      // Check if roi width is acceptable (ie roi is contained in the image)
      if (w > (max_width - left))
        w = (max_width - left);
      roi_width = (int32_t)w;
    }
    else {
      roi_width = DC1394_USE_MAX_AVAIL;
    }
 
    if (h != 0) {
      // Check if roi height is acceptable (ie roi is contained in the image)
      if (h > (max_height - top))
        h = (max_height - top);
      roi_height = (int32_t)h;
    }
    else {
      roi_height = DC1394_USE_MAX_AVAIL;
    }
 
    if (dc1394_format7_set_roi(camera, _videomode,
                               (dc1394color_coding_t)DC1394_QUERY_FROM_CAMERA, // color_coding
                               DC1394_USE_MAX_AVAIL                            /*DC1394_QUERY_FROM_CAMERA*/
                               ,                                               // bytes_per_packet
                               (int32_t)left,                                  // left
                               (int32_t)top,                                   // top
                               roi_width, roi_height) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Can't set format7 roi");
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get current video mode"));
    }
    // Update the image size
    if (dc1394_format7_get_image_size(camera, _videomode, &this->width, &this->height) != DC1394_SUCCESS) {
      close();
      vpERROR_TRACE("Can't get format7 image size");
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't get format7 image size"));
    }
 
    setCapture(DC1394_ON);
    setTransmission(DC1394_ON);
  }
}

void vp1394TwoGrabber::initialize(bool reset)
{
  if (init == false) {
// Find cameras
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
    if (d != nullptr)
      dc1394_free(d);
 
    d = dc1394_new();
    if (dc1394_camera_enumerate(d, &list) != DC1394_SUCCESS) {
      dc1394_camera_free_list(list);
      close();
      vpERROR_TRACE("Failed to enumerate cameras\n");
      throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Failed to enumerate cameras"));
    }
 
    if (list->num == 0) {
      dc1394_camera_free_list(list);
      close();
      vpERROR_TRACE("No cameras found");
      throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found"));
    }
 
    if (cameras != nullptr)
      delete[] cameras;
 
    cameras = new dc1394camera_t *[list->num];
 
    num_cameras = 0;
 
    for (unsigned int i = 0; i < list->num; i++) {
      cameras[i] = dc1394_camera_new(d, list->ids[i].guid);
      if (!cameras[i]) {
        vpTRACE("Failed to initialize camera with guid \"%ld\"\n", list->ids[i].guid);
        continue;
      }
      // Update the number of working cameras
      num_cameras++;
    }
 
    if (reset) {
      // Reset the bus to make firewire working if the program was not
      // properly stopped by a CTRL-C. We reset here only the bus attached to
      // the first camera
      dc1394_reset_bus(cameras[0]);
    }
 
    // if (list != nullptr)
    dc1394_camera_free_list(list);
    list = nullptr;
 
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
    if (cameras != nullptr)
      free(cameras);
    cameras = nullptr;
    int err = dc1394_find_cameras(&cameras, &num_cameras);
 
    if (err != DC1394_SUCCESS && err != DC1394_NO_CAMERA) {
      close();
      vpERROR_TRACE("Unable to look for cameras\n\n"
                    "Please check \n"
                    "  - if the kernel modules `ieee1394',`raw1394' and "
                    "`ohci1394' are loaded \n"
                    "  - if you have read/write access to /dev/raw1394\n\n");
      throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Unable to look for cameras"));
    }
#endif
 
    if (num_cameras == 0) {
      close();
      vpERROR_TRACE("No cameras found");
      throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found"));
    }
 
    // allocation for the parameters
    isDataModified = new bool[num_cameras];
    for (unsigned int i = 0; i < num_cameras; i++)
      isDataModified[i] = false;
    initialShutterMode = new dc1394feature_mode_t[num_cameras];
    dataCam = new vpDc1394TwoCameraParametersData[num_cameras];
 
    if (camera_id >= num_cameras) {
      // Bad camera id
      close();
      vpERROR_TRACE("Bad camera id: %u", camera_id);
      vpERROR_TRACE("Only %u camera on the bus.", num_cameras);
      throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Bad camera id"));
    }
 
    if (verbose) {
      std::cout << "------ Bus information ------" << std::endl;
      std::cout << "Number of camera(s) on the bus : " << num_cameras << std::endl;
      std::cout << "-----------------------------" << std::endl;
    }
 
    if (camIsOpen != nullptr)
      delete[] camIsOpen;
    camIsOpen = new bool[num_cameras];
    for (unsigned int i = 0; i < num_cameras; i++) {
      camIsOpen[i] = false;
    }
 
    init = true;
  }
}
void vp1394TwoGrabber::open()
{
  if (init == false)
    initialize(false);
  if (camIsOpen[camera_id] == false) {
    dc1394switch_t status = DC1394_OFF;
 
    //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API >
    // libdc1394-2.0.0-rc7
    dc1394_video_get_transmission(cameras[camera_id], &status);
    if (status != DC1394_OFF) {
      //#endif
      if (dc1394_video_set_transmission(cameras[camera_id], DC1394_OFF) != DC1394_SUCCESS)
        vpTRACE("Could not stop ISO transmission");
      else {
        vpTime::wait(500);
        if (dc1394_video_get_transmission(cameras[camera_id], &status) != DC1394_SUCCESS)
          vpTRACE("Could get ISO status");
        else {
          if (status == DC1394_ON) {
            vpTRACE("ISO transmission refuses to stop");
          }
#ifdef VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
          // No yet in the new API
          cameras[camera_id]->is_iso_on = status;
#endif
        }
        //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API >
        // libdc1394-2.0.0-rc7
      }
      //#endif
    }
    setCamera(camera_id);
    // setIsoSpeed(DC1394_ISO_SPEED_400);
    setCapture(DC1394_ON);
    setTransmission(DC1394_ON);
    camIsOpen[camera_id] = true;
  }
}
void vp1394TwoGrabber::close()
{
  if (init) {
    if (num_cameras) {
      for (unsigned int i = 0; i < num_cameras; i++) {
        if (camIsOpen[i]) {
          camera = cameras[i];
          this->camera_id = i; // set camera id for the function updateDataStructToCam
          setTransmission(DC1394_OFF);
          setCapture(DC1394_OFF);
          if (isDataModified[i]) {
            // reset values
            try {
              updateDataStructToCam();
            }
            catch (...) {
            }
            // reset mode (manual, auto, ...)
            if (dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_EXPOSURE, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_SHARPNESS, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_HUE, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_SATURATION, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_GAMMA, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, initialShutterMode[i]) != DC1394_SUCCESS ||
                dc1394_feature_set_mode(camera, DC1394_FEATURE_IRIS, initialShutterMode[i])) {
 
              vpERROR_TRACE("Unable to reset the initial mode");
              throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to reset the initial mode"));
            }
          }
          if (dc1394_camera_set_power(camera, DC1394_OFF) != DC1394_SUCCESS)
            std::cout << "Unable to turn camera off" << std::endl;
        }
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
        dc1394_camera_free(cameras[i]);
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
        dc1394_free_camera(cameras[i]);
#endif
      }
    }
    if (camIsOpen != nullptr) {
      delete[] camIsOpen;
      camIsOpen = nullptr;
    }
 
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
    if (cameras != nullptr) {
      delete[] cameras;
      cameras = nullptr;
    }
    if (d != nullptr) {
      dc1394_free(d);
      d = nullptr;
    }
 
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
    if (cameras != nullptr) {
      free(cameras);
      cameras = nullptr;
    }
#endif
 
    camIsOpen = nullptr;
    num_cameras = 0;
 
    // remove data for the parameters
    if (isDataModified != nullptr) {
      delete[] isDataModified;
      isDataModified = nullptr;
    }
    if (initialShutterMode != nullptr) {
      delete[] initialShutterMode;
      initialShutterMode = nullptr;
    }
    if (dataCam != nullptr) {
      delete[] dataCam;
      dataCam = nullptr;
    }
 
    init = false;
  }
}

void vp1394TwoGrabber::setRingBufferSize(unsigned int size)
{
  if (size < 1) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not set ring buffer size"));
  }
 
  if (size != num_buffers) {
    // We need to change the ring buffer size
    num_buffers = size;
    if (camIsOpen[camera_id]) {
      setCapture(DC1394_OFF);
      setCapture(DC1394_ON);
    }
  }
}

unsigned int vp1394TwoGrabber::getRingBufferSize() const { return num_buffers; }

void vp1394TwoGrabber::setAutoShutter(bool enable)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394feature_mode_t mode;
  if (enable) {
    mode = DC1394_FEATURE_MODE_AUTO;
  }
  else {
    mode = DC1394_FEATURE_MODE_MANUAL;
  }
 
  if (dc1394_feature_set_power(camera, DC1394_FEATURE_SHUTTER, DC1394_ON) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set shutter on. \n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set shutter on"));
  }
 
  if (dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, mode) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set auto shutter. \n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto shutter"));
  }
}

void vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, unsigned int maxvalue)
{
  setAutoShutter();
 
  if (dc1394_avt_set_auto_shutter(camera, minvalue, maxvalue) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set auto shutter min and max values. Is the
    //       camera an AVT one?\n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto shutter min and max values"));
  }
}

void vp1394TwoGrabber::getAutoShutter(unsigned int &minvalue, unsigned int &maxvalue)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  if (dc1394_avt_get_auto_shutter(camera, &minvalue, &maxvalue) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot get auto shutter min and max values. Is the
    //       camera an AVT one?\n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot get auto shutter min and max values"));
  }
}

void vp1394TwoGrabber::setAutoGain(bool enable)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394feature_mode_t mode;
  if (enable) {
    mode = DC1394_FEATURE_MODE_AUTO;
  }
  else {
    mode = DC1394_FEATURE_MODE_MANUAL;
  }
 
  if (dc1394_feature_set_power(camera, DC1394_FEATURE_GAIN, DC1394_ON) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set shutter on. \n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set shutter on"));
  }
 
  if (dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, mode) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set auto gain. \n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto gain"));
  }
}

void vp1394TwoGrabber::setAutoGain(unsigned int minvalue, unsigned int maxvalue)
{
  setAutoGain();
 
  if (dc1394_avt_set_auto_gain(camera, minvalue, maxvalue) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set auto gain min and max values. Is the
    //       camera an AVT one?\n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set auto gain min and max values"));
  }
}

void vp1394TwoGrabber::getAutoGain(unsigned int &minvalue, unsigned int &maxvalue)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  if (dc1394_avt_get_auto_gain(camera, &minvalue, &maxvalue) != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot get auto gain min and max values. Is the
    //       camera an AVT one?\n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot get auto gain min and max values"));
  }
}

void vp1394TwoGrabber::setCapture(dc1394switch_t _switch)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  if (_switch == DC1394_ON) {
    // if (dc1394_capture_setup(camera, num_buffers) != DC1394_SUCCESS) {
    // To be compatible with libdc1394 svn 382 version
    if (dc1394_capture_setup(camera, num_buffers, DC1394_CAPTURE_FLAGS_DEFAULT) != DC1394_SUCCESS) {
      vpERROR_TRACE("Unable to setup camera capture-\n"
                    "make sure that the video mode and framerate are "
                    "supported by your camera.\n");
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture"));
    }
  }
  else { // _switch == DC1394_OFF
    dc1394error_t code = dc1394_capture_stop(camera);
 
    if (code != DC1394_SUCCESS && code != DC1394_CAPTURE_IS_NOT_SET) {
      vpERROR_TRACE("Unable to stop camera capture\n");
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture"));
    }
  }
}

void vp1394TwoGrabber::setTransmission(dc1394switch_t _switch)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394switch_t status = DC1394_OFF;
 
  if (dc1394_video_get_transmission(camera, &status) != DC1394_SUCCESS) {
    vpERROR_TRACE("Unable to get transmision status");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture"));
  }
 
  //    if (status!=_switch){
  // Start dma capture if halted
  if (dc1394_video_set_transmission(camera, _switch) != DC1394_SUCCESS) {
    vpERROR_TRACE("Unable to setup camera capture-\n"
                  "make sure that the video mode and framerate are "
                  "supported by your camera.\n");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture"));
  }
 
  if (_switch == DC1394_ON) {
    status = DC1394_OFF;
 
    int i = 0;
    while (status == DC1394_OFF && i++ < 5) {
      usleep(50000);
      if (dc1394_video_get_transmission(camera, &status) != DC1394_SUCCESS) {
        vpERROR_TRACE("Unable to get transmision status");
        close();
        throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture"));
      }
    }
  }
  //    }
}

void vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394operation_mode_t op_mode;
  dc1394speed_t speed;
 
  // Check the speed to configure in B-mode or A-mode
  if (isospeed >= vpISO_SPEED_800) {
    if (camera->bmode_capable != DC1394_TRUE) {
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Camera is not 1394B mode capable"));
    }
 
    if (dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B) != DC1394_SUCCESS) {
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set camera to 1394B mode"));
    }
 
    if (dc1394_video_get_operation_mode(camera, &op_mode) != DC1394_SUCCESS) {
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set 1394B mode"));
    }
  }
  else {
    if (dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_LEGACY) != DC1394_SUCCESS) {
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set camera to 1394A mode"));
    }
 
    if (dc1394_video_get_operation_mode(camera, &op_mode) != DC1394_SUCCESS) {
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set 1394A mode"));
    }
  }
 
  if (dc1394_video_set_iso_speed(camera, (dc1394speed_t)isospeed) != DC1394_SUCCESS) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set requested iso speed"));
  }
 
  if (dc1394_video_get_iso_speed(camera, &speed) != DC1394_SUCCESS) {
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Failed to set iso speed"));
  }
}

void vp1394TwoGrabber::open(vpImage<unsigned char> &I)
{
  open();
  acquire(I);
}

void vp1394TwoGrabber::open(vpImage<vpRGBa> &I)
{
  open();
  acquire(I);
}

dc1394video_frame_t *vp1394TwoGrabber::dequeue()
{
 
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  dc1394video_frame_t *frame = nullptr;
 
  if (dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame) != DC1394_SUCCESS) {
    vpERROR_TRACE("Error: Failed to capture from camera %d\n", camera_id);
  }
 
  return frame;
}

dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I)
{
  uint64_t timestamp;
  uint32_t id;
 
  dc1394video_frame_t *frame;
 
  frame = dequeue(I, timestamp, id);
 
  return frame;
}

dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I, uint64_t &timestamp, uint32_t &id)
{
 
  open();
 
  dc1394video_frame_t *frame;
 
  frame = dequeue();
 
  // Timeval data structure providing the unix time
  // [microseconds] at which the frame was captured in the ring buffer.
  timestamp = frame->timestamp;
  id = frame->id;
 
  this->width = frame->size[0];
  this->height = frame->size[1];
  unsigned int size = this->width * this->height;
 
  if ((I.getWidth() != this->width) || (I.getHeight() != this->height))
    I.resize(this->height, this->width);
 
  switch (frame->color_coding) {
  case DC1394_COLOR_CODING_MONO8:
  case DC1394_COLOR_CODING_RAW8:
    memcpy(I.bitmap, (unsigned char *)frame->image, size * sizeof(unsigned char));
    break;
  case DC1394_COLOR_CODING_MONO16:
  case DC1394_COLOR_CODING_RAW16:
    vpImageConvert::MONO16ToGrey((unsigned char *)frame->image, I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV411:
    vpImageConvert::YUV411ToGrey((unsigned char *)frame->image, I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV422:
    vpImageConvert::YUV422ToGrey((unsigned char *)frame->image, I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV444:
    vpImageConvert::YUV444ToGrey((unsigned char *)frame->image, I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_RGB8:
    vpImageConvert::RGBToGrey((unsigned char *)frame->image, I.bitmap, size);
    break;
 
  default:
    close();
    vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
    throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
                                  "Acquisition failed."));
  };
 
  return frame;
}

dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I)
{
  uint64_t timestamp;
  uint32_t id;
 
  dc1394video_frame_t *frame;
 
  frame = dequeue(I, timestamp, id);
 
  return frame;
}

dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I, uint64_t &timestamp, uint32_t &id)
{
 
  open();
 
  dc1394video_frame_t *frame;
 
  frame = dequeue();
 
  // Timeval data structure providing the unix time
  // [microseconds] at which the frame was captured in the ring buffer.
  timestamp = frame->timestamp;
  id = frame->id;
 
  this->width = frame->size[0];
  this->height = frame->size[1];
  unsigned int size = this->width * this->height;
 
  if ((I.getWidth() != width) || (I.getHeight() != height))
    I.resize(height, width);
 
  switch (frame->color_coding) {
  case DC1394_COLOR_CODING_MONO8:
  case DC1394_COLOR_CODING_RAW8:
    vpImageConvert::GreyToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_MONO16:
  case DC1394_COLOR_CODING_RAW16:
    vpImageConvert::MONO16ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV411:
    vpImageConvert::YUV411ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV422:
    vpImageConvert::YUV422ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV444:
    vpImageConvert::YUV444ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_RGB8:
    vpImageConvert::RGBToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  default:
    close();
    vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
    throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
                                  "Acquisition failed."));
  };
 
  return frame;
}

void vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame)
{
 
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  if (frame)
    dc1394_capture_enqueue(camera, frame);
}

void vp1394TwoGrabber::acquire(vpImage<unsigned char> &I)
{
  uint64_t timestamp;
  uint32_t id;
 
  dc1394video_frame_t *frame;
 
  frame = dequeue(I, timestamp, id);
  enqueue(frame);
}

void vp1394TwoGrabber::acquire(vpImage<unsigned char> &I, uint64_t &timestamp, uint32_t &id)
{
  dc1394video_frame_t *frame;
 
  open();
  frame = dequeue(I, timestamp, id);
  enqueue(frame);
}

void vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I)
{
  uint64_t timestamp;
  uint32_t id;
  dc1394video_frame_t *frame;
 
  open();
  frame = dequeue(I, timestamp, id);
  enqueue(frame);
}

void vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I, uint64_t &timestamp, uint32_t &id)
{
  dc1394video_frame_t *frame;
 
  open();
  frame = dequeue();
  // Timeval data structure providing the unix time
  // [microseconds] at which the frame was captured in the ring buffer.
  timestamp = frame->timestamp;
  id = frame->id;
 
  this->width = frame->size[0];
  this->height = frame->size[1];
  unsigned int size = this->width * this->height;
 
  if ((I.getWidth() != width) || (I.getHeight() != height))
    I.resize(height, width);
 
  switch (frame->color_coding) {
  case DC1394_COLOR_CODING_MONO8:
  case DC1394_COLOR_CODING_RAW8:
    vpImageConvert::GreyToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV411:
    vpImageConvert::YUV411ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV422:
    vpImageConvert::YUV422ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_YUV444:
    vpImageConvert::YUV444ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  case DC1394_COLOR_CODING_RGB8:
    vpImageConvert::RGBToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
    break;
 
  default:
    close();
    vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
    throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
                                  "Acquisition failed."));
  };
 
  enqueue(frame);
}

void vp1394TwoGrabber::getWidth(unsigned int &w)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  w = this->width;
}

unsigned int vp1394TwoGrabber::getWidth()
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  return this->width;
}

void vp1394TwoGrabber::getHeight(unsigned int &h)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  h = this->height;
}

unsigned int vp1394TwoGrabber::getHeight()
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  return this->height;
}

void vp1394TwoGrabber::printCameraInfo()
{
  std::cout << "----------------------------------------------------------" << std::endl
    << "-----            Information for camera " << camera_id << "            -----" << std::endl
    << "----------------------------------------------------------" << std::endl;
 
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  dc1394_camera_print_info(camera, stdout);
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
  dc1394_print_camera_info(camera);
#endif
 
  dc1394featureset_t features;
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  if (dc1394_feature_get_all(camera, &features) != DC1394_SUCCESS)
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
  if (dc1394_get_camera_feature_set(camera, &features) != DC1394_SUCCESS)
#endif
  {
    close();
    vpERROR_TRACE("unable to get feature set for camera %d\n", camera_id);
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot get camera features"));
 
  }
  else {
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
    dc1394_feature_print_all(&features, stdout);
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
    dc1394_print_feature_set(&features);
#endif
  }
  std::cout << "----------------------------------------------------------" << std::endl;
}

std::string vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode)
{
  std::string _str = "";
  dc1394video_mode_t _videomode = (dc1394video_mode_t)videomode;
 
  if ((_videomode >= DC1394_VIDEO_MODE_MIN) && (_videomode <= DC1394_VIDEO_MODE_MAX)) {
    _str = strVideoMode[_videomode - DC1394_VIDEO_MODE_MIN];
  }
  else {
    vpCERROR << "The video mode " << static_cast<int>(videomode) << " is not supported by the camera" << std::endl;
  }
 
  return _str;
}

std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps)
{
  std::string _str = "";
  dc1394framerate_t _fps = (dc1394framerate_t)fps;
 
  if ((_fps >= DC1394_FRAMERATE_MIN) && (_fps <= DC1394_FRAMERATE_MAX)) {
    _str = strFramerate[_fps - DC1394_FRAMERATE_MIN];
  }
  else {
    vpCERROR << "The framerate " << static_cast<int>(fps) << " is not supported by the camera" << std::endl;
  }
 
  return _str;
}

std::string vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorcoding)
{
  std::string _str = "";
  dc1394color_coding_t _coding = (dc1394color_coding_t)colorcoding;
 
  if ((_coding >= DC1394_COLOR_CODING_MIN) && (_coding <= DC1394_COLOR_CODING_MAX)) {
    _str = strColorCoding[_coding - DC1394_COLOR_CODING_MIN];
 
  }
  else {
    vpCERROR << "The color coding " << static_cast<int>(colorcoding) << " is not supported by the camera" << std::endl;
  }
 
  return _str;
}

vp1394TwoGrabber::vp1394TwoVideoModeType vp1394TwoGrabber::string2videoMode(std::string videomode)
{
  vp1394TwoVideoModeType _id;
 
  for (int i = DC1394_VIDEO_MODE_MIN; i <= DC1394_VIDEO_MODE_MAX; i++) {
    _id = (vp1394TwoVideoModeType)i;
    if (videomode.compare(videoMode2string(_id)) == 0)
      return _id;
  };
 
  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required videomode is not valid"));
 
  return (vp1394TwoVideoModeType)0;
}

vp1394TwoGrabber::vp1394TwoFramerateType vp1394TwoGrabber::string2framerate(std::string framerate)
{
  vp1394TwoFramerateType _id;
 
  for (int i = DC1394_FRAMERATE_MIN; i <= DC1394_FRAMERATE_MAX; i++) {
    _id = (vp1394TwoFramerateType)i;
    if (framerate.compare(framerate2string(_id)) == 0)
      return _id;
  };
 
  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required framerate is not valid"));
 
  return (vp1394TwoFramerateType)0;
}

vp1394TwoGrabber::vp1394TwoColorCodingType vp1394TwoGrabber::string2colorCoding(std::string colorcoding)
{
  vp1394TwoColorCodingType _id;
 
  for (int i = DC1394_COLOR_CODING_MIN; i <= DC1394_COLOR_CODING_MAX; i++) {
    _id = (vp1394TwoColorCodingType)i;
    if (colorcoding.compare(colorCoding2string(_id)) == 0)
      return _id;
  };
 
  throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required color coding is not valid"));
 
  return (vp1394TwoColorCodingType)0;
}

void vp1394TwoGrabber::resetBus()
{
  for (unsigned int i = 0; i < num_cameras; i++) {
    if (camIsOpen[i]) {
      camera = cameras[i];
      setTransmission(DC1394_OFF);
      setCapture(DC1394_OFF);
    }
  }
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  setCamera(camera_id);
  // free the other cameras
  for (unsigned int i = 0; i < num_cameras; i++) {
    if (i != camera_id)
      dc1394_camera_free(cameras[i]);
  }
 
  printf("Resetting bus...\n");
  dc1394_reset_bus(camera);
 
  dc1394_camera_free(camera);
  dc1394_free(d);
  d = nullptr;
  // if (cameras != nullptr)
  delete[] cameras;
  cameras = nullptr;
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
 
  setCamera(camera_id);
  // free the other cameras
  for (unsigned int i = 0; i < num_cameras; i++) {
    if (i != camera_id)
      dc1394_free_camera(cameras[i]);
  }
  free(cameras);
  cameras = nullptr;
 
  dc1394_reset_bus(camera);
  dc1394_free_camera(camera);
 
#endif
  if (camIsOpen != nullptr)
    delete[] camIsOpen;
  camIsOpen = nullptr;
 
  num_cameras = 0;
 
  init = false;
  vpTime::wait(1000);
  initialize(false);
}

void vp1394TwoGrabber::setPanControl(unsigned int panControlValue)
{
  open();
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  uint64_t offset = 0x884;
  uint32_t value = 0x82000000 + static_cast<uint32_t>(panControlValue);
  dc1394error_t err;
  err = dc1394_set_control_register(camera, offset, value);
  if (err != DC1394_SUCCESS) {
    vpERROR_TRACE("Unable to set PAN register");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to set PAN register"));
  }
}

unsigned int vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  uint32_t value;
  dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param;
  switch (param) {
  case vpFEATURE_BRIGHTNESS:
    feature = DC1394_FEATURE_BRIGHTNESS;
    break;
  case vpFEATURE_EXPOSURE:
    feature = DC1394_FEATURE_EXPOSURE;
    break;
  case vpFEATURE_SHARPNESS:
    feature = DC1394_FEATURE_SHARPNESS;
    break;
  //    vpFEATURE_WHITE_BALANCE   = DC1394_FEATURE_WHITE_BALANCE,
  case vpFEATURE_HUE:
    feature = DC1394_FEATURE_HUE;
    break;
  case vpFEATURE_SATURATION:
    feature = DC1394_FEATURE_SATURATION;
    break;
  case vpFEATURE_GAMMA:
    feature = DC1394_FEATURE_GAMMA;
    break;
  case vpFEATURE_SHUTTER:
    feature = DC1394_FEATURE_SHUTTER;
    break;
  case vpFEATURE_GAIN:
    feature = DC1394_FEATURE_GAIN;
    break;
  case vpFEATURE_IRIS:
    feature = DC1394_FEATURE_IRIS;
    break;
    //    vpFEATURE_FOCUS           = DC1394_FEATURE_FOCUS,
    //    vpFEATURE_TEMPERATURE     = DC1394_FEATURE_TEMPERATURE,
    //    vpFEATURE_TRIGGER         = DC1394_FEATURE_TRIGGER,
    //    vpFEATURE_TRIGGER_DELAY   = DC1394_FEATURE_TRIGGER_DELAY,
    //    vpFEATURE_WHITE_SHADING   = DC1394_FEATURE_WHITE_SHADING,
    //    vpFEATURE_FRAME_RATE      = DC1394_FEATURE_FRAME_RATE,
    //    vpFEATURE_ZOOM            = DC1394_FEATURE_ZOOM,
    //    vpFEATURE_PAN             = DC1394_FEATURE_PAN,
    //    vpFEATURE_TILT            = DC1394_FEATURE_TILT,
    //    vpFEATURE_OPTICAL_FILTER  = DC1394_FEATURE_OPTICAL_FILTER,
    //    vpFEATURE_CAPTURE_SIZE    = DC1394_FEATURE_CAPTURE_SIZE,
    //    vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
  }
 
  dc1394error_t err;
  err = dc1394_feature_get_value(camera, feature, &value);
  if (err != DC1394_SUCCESS) {
    vpERROR_TRACE("Unable to get the information");
    close();
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to get the information"));
  }
  return (static_cast<unsigned int>(value));
}

void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, unsigned int val)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
  uint32_t value = static_cast<uint32_t>(val);
  dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param;
  switch (param) {
  case vpFEATURE_BRIGHTNESS:
    feature = DC1394_FEATURE_BRIGHTNESS;
    break;
  case vpFEATURE_EXPOSURE:
    feature = DC1394_FEATURE_EXPOSURE;
    break;
  case vpFEATURE_SHARPNESS:
    feature = DC1394_FEATURE_SHARPNESS;
    break;
  //    vpFEATURE_WHITE_BALANCE   = DC1394_FEATURE_WHITE_BALANCE,
  case vpFEATURE_HUE:
    feature = DC1394_FEATURE_HUE;
    break;
  case vpFEATURE_SATURATION:
    feature = DC1394_FEATURE_SATURATION;
    break;
  case vpFEATURE_GAMMA:
    feature = DC1394_FEATURE_GAMMA;
    break;
  case vpFEATURE_SHUTTER:
    feature = DC1394_FEATURE_SHUTTER;
    break;
  case vpFEATURE_GAIN:
    feature = DC1394_FEATURE_GAIN;
    break;
  case vpFEATURE_IRIS:
    feature = DC1394_FEATURE_IRIS;
    break;
    //    vpFEATURE_FOCUS           = DC1394_FEATURE_FOCUS,
    //    vpFEATURE_TEMPERATURE     = DC1394_FEATURE_TEMPERATURE,
    //    vpFEATURE_TRIGGER         = DC1394_FEATURE_TRIGGER,
    //    vpFEATURE_TRIGGER_DELAY   = DC1394_FEATURE_TRIGGER_DELAY,
    //    vpFEATURE_WHITE_SHADING   = DC1394_FEATURE_WHITE_SHADING,
    //    vpFEATURE_FRAME_RATE      = DC1394_FEATURE_FRAME_RATE,
    //    vpFEATURE_ZOOM            = DC1394_FEATURE_ZOOM,
    //    vpFEATURE_PAN             = DC1394_FEATURE_PAN,
    //    vpFEATURE_TILT            = DC1394_FEATURE_TILT,
    //    vpFEATURE_OPTICAL_FILTER  = DC1394_FEATURE_OPTICAL_FILTER,
    //    vpFEATURE_CAPTURE_SIZE    = DC1394_FEATURE_CAPTURE_SIZE,
    //    vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
  }
 
  dc1394error_t err;
  dc1394bool_t hasManualMode = DC1394_FALSE;
  dc1394feature_modes_t modesAvailable;
 
  // test wether we can set the shutter value (manual mode available or not)
  err = dc1394_feature_get_modes(camera, feature, &modesAvailable);
  if (err != DC1394_SUCCESS) {
    vpERROR_TRACE("Unable to detect the manual mode information");
    close();
    throw(
        vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to detect the manual mode information"));
  }
 
  for (unsigned int i = 0; i < modesAvailable.num; i++) {
    if (modesAvailable.modes[i] == DC1394_FEATURE_MODE_MANUAL) {
      hasManualMode = DC1394_TRUE;
    }
  }
 
  if (hasManualMode == DC1394_TRUE) {
 
    if (!isDataModified[camera_id]) { //  to ensure we save the first mode
                                      //  even after several set
      /* we update the structure */
      updateDataCamToStruct();
      err = dc1394_feature_get_mode(camera, feature, &(initialShutterMode[camera_id]));
      if (err != DC1394_SUCCESS) {
        vpERROR_TRACE("Unable to get the initial mode");
        close();
        throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to get the initial mode"));
      }
      isDataModified[camera_id] = true;
    }
 
    dc1394feature_mode_t manualMode = DC1394_FEATURE_MODE_MANUAL;
    err = dc1394_feature_set_mode(camera, feature, manualMode);
    if (err != DC1394_SUCCESS) {
      vpERROR_TRACE("Unable to set the muanual mode");
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to set the manual mode"));
    }
    err = dc1394_feature_set_value(camera, feature, value);
    if (err != DC1394_SUCCESS) {
      vpERROR_TRACE("Unable to set the shutter information");
      close();
      throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to set the shutter information"));
    }
  }
  else {
    vpERROR_TRACE("The camera does not have a manual mode.\nCannot change the value");
    throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The camera does not have a manual mode"));
  }
}

void vp1394TwoGrabber::getGuid(uint64_t &guid)
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  guid = camera->guid;
}

uint64_t vp1394TwoGrabber::getGuid()
{
  if (!num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
  }
 
  return camera->guid;
}

inline void vp1394TwoGrabber::updateDataCamToStruct()
{
  dataCam[camera_id].brightness = getParameterValue(vpFEATURE_BRIGHTNESS);
  dataCam[camera_id].exposure = getParameterValue(vpFEATURE_EXPOSURE);
  dataCam[camera_id].sharpness = getParameterValue(vpFEATURE_SHARPNESS);
  dataCam[camera_id].hue = getParameterValue(vpFEATURE_HUE);
  dataCam[camera_id].saturation = getParameterValue(vpFEATURE_SATURATION);
  dataCam[camera_id].gamma = getParameterValue(vpFEATURE_GAMMA);
  dataCam[camera_id].shutter = getParameterValue(vpFEATURE_SHUTTER);
  dataCam[camera_id].gain = getParameterValue(vpFEATURE_GAIN);
  dataCam[camera_id].iris = getParameterValue(vpFEATURE_IRIS);
}

inline void vp1394TwoGrabber::updateDataStructToCam()
{
  setParameterValue(vpFEATURE_BRIGHTNESS, dataCam[camera_id].brightness);
  setParameterValue(vpFEATURE_EXPOSURE, dataCam[camera_id].exposure);
  setParameterValue(vpFEATURE_SHARPNESS, dataCam[camera_id].sharpness);
  setParameterValue(vpFEATURE_HUE, dataCam[camera_id].hue);
  setParameterValue(vpFEATURE_SATURATION, dataCam[camera_id].saturation);
  setParameterValue(vpFEATURE_GAMMA, dataCam[camera_id].gamma);
  setParameterValue(vpFEATURE_SHUTTER, dataCam[camera_id].shutter);
  setParameterValue(vpFEATURE_GAIN, dataCam[camera_id].gain);
  setParameterValue(vpFEATURE_IRIS, dataCam[camera_id].iris);
}

vp1394TwoGrabber &vp1394TwoGrabber::operator>>(vpImage<unsigned char> &I)
{
  this->acquire(I);
  return *this;
}

vp1394TwoGrabber &vp1394TwoGrabber::operator>>(vpImage<vpRGBa> &I)
{
  this->acquire(I);
  return *this;
}
END_VISP_NAMESPACE
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work around to avoid warning: libvisp_sensor.a(vp1394TwoGrabber.cpp.o) has
// no symbols
void dummy_vp1394TwoGrabber() { }
#endif