vp1394TwoGrabber.cpp

/****************************************************************************
 *
 * $Id: vp1394TwoGrabber.cpp 4056 2013-01-05 13:04:42Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2013 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
 * ("GPL") version 2 as published by the Free Software Foundation.
 * 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 http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * 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.
 *
 * Authors:
 * Fabien Spindler
 *
 *****************************************************************************/


#include <iostream>

#include <visp/vpConfig.h>

/*
 * Interface with libdc1394 2.x
 */
#if defined(VISP_HAVE_DC1394_2)
#include <unistd.h>

#include <visp/vp1394TwoGrabber.h>
#include <visp/vpFrameGrabberException.h>
#include <visp/vpImageIo.h>
#include <visp/vpImageConvert.h>
#include <visp/vpTime.h>

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)
{
  // protected members
  width = height = 0;

  // private members
  num_cameras = 0;
  cameras = NULL;
  camera_id = 0;
  verbose = false;//true;
  camIsOpen = NULL;
  init = false;
  cameras = NULL;
#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  d = NULL;
  list = NULL;
#endif
  num_buffers = 4; // ring buffer size

  isDataModified = NULL; 
  initialShutterMode = NULL;
  dataCam = NULL; 
  
  reset = false;
  initialize(reset);
  
  //  open();
}

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

void
vp1394TwoGrabber::setCamera(uint64_t camera_id)
{
  // Suppose that if camera_id is a camera GUID, this value is greater
  // than the number of cameras connected to the bus
  if (camera_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 == camera_id) {
    this->camera_id = i; 
    is_guid = true;
    break;
      }
    }
    if (is_guid == false) {
      close();
      vpERROR_TRACE("The camera with GUID 0x%x or id %u is not present", 
            camera_id, camera_id);
      vpERROR_TRACE("Only %u camera on the bus.", num_cameras);
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "The required camera is not present") );
    }
  }
  else {
    this->camera_id =  camera_id;
  }

  // create a pointer to the working camera
  camera = cameras[this->camera_id];
}


void
vp1394TwoGrabber::getCamera(uint64_t &camera_id)
{
  if (num_cameras) {
    camera_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)
{
  if (! num_cameras) {
    vpCTRACE << "No camera found..."<< std::endl;
    ncameras = 0;
  }

  ncameras = num_cameras;
}

unsigned int
vp1394TwoGrabber::getNumCameras()
{
  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();
    vpERROR_TRACE("No camera found");
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
                                   "No camera found") );
  }
  if (!isVideoModeSupported(videomode)){
    vpERROR_TRACE("Video mode not supported by camera %d",camera_id);
    throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                                   "Video mode not supported") );
    return ;
  }
  // Stop dma capture if started
  setTransmission(DC1394_OFF);
  setCapture(DC1394_OFF);

  if (dc1394_video_set_mode(camera, (dc1394video_mode_t) videomode) != DC1394_SUCCESS) {

    close();
    vpERROR_TRACE("Can't set video mode");
    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();
    vpERROR_TRACE("Can't set video mode");
    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();
    vpERROR_TRACE("No camera found");
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
                                   "No camera found") );
  }

  vp1394TwoVideoModeType cur_videomode;
  getVideoMode(cur_videomode);
  if (isVideoModeFormat7(cur_videomode))
    return;

  if (!isFramerateSupported(cur_videomode,fps)){
    vpERROR_TRACE("Framerate not supported by camera %d",camera_id);
    throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                                   "Framerate not supported") );
    return ;
  }

  // Stop dma capture if started
  setTransmission(DC1394_OFF);
  setCapture(DC1394_OFF);

  if (dc1394_video_set_framerate(camera, (dc1394framerate_t) fps) != DC1394_SUCCESS) {

    close();
    vpERROR_TRACE("Can't set framerate");
    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();
    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 (!isColorCodingSupported((vp1394TwoVideoModeType)_videomode,coding)){
    vpERROR_TRACE("Color coding not supported by camera %d",camera_id);
    throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                                   "Color coding not supported") );
    return ;
  }

  // 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();
      vpERROR_TRACE("Can't set color coding");
      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 width, unsigned int height)
{
  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: width,
            height == 0 ? DC1394_USE_MAX_AVAIL : height);
    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 (width != 0) {
      // Check if roi width is acceptable (ie roi is contained in the image)
      if (width > (max_width - left))
        width = (max_width - left);
      roi_width = (int32_t)width;
    }
    else {
      roi_width = DC1394_USE_MAX_AVAIL;
    }

    if (height != 0) {
      // Check if roi height is acceptable (ie roi is contained in the image)
      if (height > (max_height - top))
        height = (max_height - top);
      roi_height = (int32_t)height;
    }
    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_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
    if (d != NULL)
      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 != NULL)
      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 != NULL)
      dc1394_camera_free_list (list);
    list = NULL;

#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
    if (cameras != NULL)
      free(cameras);
    cameras = NULL;
    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 != NULL) 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_2_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_2_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_2_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_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
        dc1394_camera_free(cameras[i]);
#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
        dc1394_free_camera(cameras[i]);
#endif
      }
    }
    if (camIsOpen != NULL) {
      delete [] camIsOpen;
      camIsOpen = NULL;
    }

#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
    if (cameras != NULL) {
      delete [] cameras;
      cameras = NULL;
    }
    if (d != NULL) {
      dc1394_free (d);
      d = NULL;
    }

#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
    if (cameras != NULL) {
      free(cameras);
      cameras = NULL;
    }
#endif

    camIsOpen = NULL;
    num_cameras = 0;
  
    // remove data for the parameters 
    if(isDataModified != NULL){
      delete[] isDataModified;
      isDataModified = NULL;
    }
    if(initialShutterMode != NULL){
      delete[] initialShutterMode;
      initialShutterMode = NULL;
    }
    if(dataCam != NULL){
      delete[] dataCam;
      dataCam = NULL;
    }   

    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()
{
  return num_buffers;
}

void
vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, unsigned int maxvalue)
{
  if (! num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
                                   "No camera found") );
  }
  
  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,
                  DC1394_FEATURE_MODE_AUTO)
      != DC1394_SUCCESS) {
    //       vpERROR_TRACE("Cannot set auto shutter. \n");
    close();
    throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                   "Cannot set auto shutter") );
  }

  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::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) {
//       vpERROR_TRACE("Camera is not 1394B mode capable. \n"
//          "Set the iso speed lower or equal to 400Mbps");
      close();
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "Camera is not 1394B mode capable") );
    }

    if(dc1394_video_set_operation_mode(camera,
                       DC1394_OPERATION_MODE_1394B)
       != DC1394_SUCCESS) {
//       vpERROR_TRACE("Cannot set camera to 1394B mode. \n");
      close();
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "Cannot set camera to 1394B mode") );
    }

    if (dc1394_video_get_operation_mode(camera, &op_mode) != DC1394_SUCCESS) {
//       vpERROR_TRACE("Failed to set 1394B mode. \n");
      close();
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "Failed to set 1394B mode") );
    }
  }
  else {
    if (dc1394_video_set_operation_mode(camera,
                    DC1394_OPERATION_MODE_LEGACY) 
    != DC1394_SUCCESS) {
//       vpERROR_TRACE("Cannot set camera to 1394A mode. \n");
      close();
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "Cannot set camera to 1394A mode") );
    }
       
    if (dc1394_video_get_operation_mode(camera, &op_mode) != DC1394_SUCCESS) {
//       vpERROR_TRACE("Failed to set 1394A mode. \n");
      close();
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "Failed to set 1394A mode") );
    } 
  }

  if (dc1394_video_set_iso_speed(camera, (dc1394speed_t) isospeed) 
    != DC1394_SUCCESS) {
//       vpERROR_TRACE("Cannot set requested iso speed. \n");
      close();
      throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
                     "Cannot set requested iso speed") );
  }

  if (dc1394_video_get_iso_speed(camera, &speed) != DC1394_SUCCESS) {
//       vpERROR_TRACE("Failed to set iso speed. \n");
    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 = NULL;

  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.") );
      break;
  };

  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.") );
      break;
  };

  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.") );
      break;
  };

  enqueue(frame);

}

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

  width = 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 &height)
{
  if (! num_cameras) {
    close();
    vpERROR_TRACE("No camera found");
    throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
                                   "No camera found") );
  }

  height = 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_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  dc1394_camera_print_info( camera, stdout);
#elif defined VISP_HAVE_DC1394_2_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
  dc1394_print_camera_info( camera);
#endif

  dc1394featureset_t features;
#ifdef VISP_HAVE_DC1394_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
  if (dc1394_feature_get_all(camera, &features) != DC1394_SUCCESS) {
#elif defined VISP_HAVE_DC1394_2_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_2_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
    dc1394_feature_print_all(&features, stdout);
#elif defined VISP_HAVE_DC1394_2_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 " << 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 " << 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 " << 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_2_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 ("Reseting bus...\n");
  dc1394_reset_bus (camera);

  dc1394_camera_free (camera);
  dc1394_free (d);
  d = NULL;
  if (cameras != NULL)
    delete [] cameras;
  cameras = NULL ;
#elif defined VISP_HAVE_DC1394_2_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 = NULL;

  dc1394_reset_bus(camera);
  dc1394_free_camera(camera);

#endif
  if (camIsOpen != NULL)
    delete [] camIsOpen;
  camIsOpen = NULL ;

  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 + (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 = (dc1394feature_t)param;
  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 (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 = (uint32_t)val;
  dc1394feature_t feature = (dc1394feature_t)param;
  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);
}

#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS

uint32_t
vp1394TwoGrabber::getVideoModeSupported(vpList<vp1394TwoVideoModeType> & videomodes)
{
  // Refresh the list of supported modes
  videomodes.kill();

  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.addRight( _mode );
  }

  // return the number of available video modes
  return _videomodes.num;
}

uint32_t
vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode,
                                        vpList<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.kill();

  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.addRight((vp1394TwoFramerateType)_fps.framerates[i]);

      return _fps.num;
    }
    break;
  }
}

uint32_t
vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
    vpList<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.kill();

  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.addRight((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.addRight((vp1394TwoColorCodingType)_coding);
    return 1;
  }
}
#endif

#endif