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

#include "hwdrivers-precomp.h"	// Precompiled headers
//
#include <mrpt/config.h>
#include <mrpt/hwdrivers/CImageGrabber_dc1394.h>

// Include the libdc1394-2 headers:
#if MRPT_HAS_LIBDC1394_2
#include <dc1394/control.h>
#include <dc1394/conversions.h>
#include <dc1394/register.h>
#include <dc1394/utils.h>
#endif

using namespace std;
using namespace mrpt;
using namespace mrpt::hwdrivers;

#define THE_CAMERA static_cast<dc1394camera_t*>(m_dc1394camera)
#define THE_CONTEXT static_cast<dc1394_t*>(m_dc1394_lib_context)

/*-------------------------------------------------------------
					Constructor
 -------------------------------------------------------------*/
CImageGrabber_dc1394::CImageGrabber_dc1394(
	uint64_t cameraGUID, uint16_t cameraUnit,
	const TCaptureOptions_dc1394& options, bool verbose)
	: m_options(options)
{
	MRPT_START

#if MRPT_HAS_LIBDC1394_2
	// Open lib:
	m_dc1394_lib_context = dc1394_new();
	ASSERT_(m_dc1394_lib_context);

	// Enumerate cameras:
	dc1394camera_list_t* list;
	dc1394error_t err;

	err = dc1394_camera_enumerate(THE_CONTEXT, &list);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Failed to enumerate cameras "
				"(Maybe your user has no rights to access IEEE1394?)."
			 << endl;
		return;
	}

	if (!list->num)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: No cameras found." << endl;
		return;
	}

	// Open the first camera or the one given by the user:
	if (!cameraGUID)
	{
		// Default: first one:
		m_dc1394camera = dc1394_camera_new(THE_CONTEXT, list->ids[0].guid);
		if (!THE_CAMERA)
		{
			dc1394_camera_free_list(list);
			cerr << "[CImageGrabber_dc1394] ERROR: Failed to initialize camera "
					"with GUID "
				 << list->ids[0].guid << "\n";
			return;
		}
	}
	else
	{
		// Look for user-given IDs:
		for (uint32_t i = 0; i < list->num; i++)
		{
			if (list->ids[i].guid == cameraGUID &&
				list->ids[i].unit == cameraUnit)
			{
				m_dc1394camera =
					dc1394_camera_new_unit(THE_CONTEXT, cameraGUID, cameraUnit);
				if (!THE_CAMERA)
				{
					dc1394_camera_free_list(list);
					cerr << "[CImageGrabber_dc1394] ERROR: Failed to "
							"initialize camera with GUID "
						 << list->ids[0].guid << "\n";
					return;
				}
				break;
			}
		}

		if (!m_dc1394camera)
		{
			dc1394_camera_free_list(list);
			cerr << "[CImageGrabber_dc1394] ERROR: Camera with GUID="
				 << cameraGUID << " and UNIT=" << cameraUnit << " not found.\n";
			return;
		}
	}
	dc1394_camera_free_list(list);

	// Camera open OK:
	if (verbose) { dc1394_camera_print_info(THE_CAMERA, stdout); }

	// get all supported modes:
	dc1394video_modes_t modes;
	err = dc1394_video_get_supported_modes(THE_CAMERA, &modes);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not get list of modes."
			 << endl;
		return;
	}

	// Is mode7? treat differently:
	if (options.mode7 >= 0)
	{
		m_desired_mode = DC1394_VIDEO_MODE_FORMAT7_MIN + options.mode7;
		if (verbose)
			cout << "[CImageGrabber_dc1394] Mode is mode7: " << options.mode7
				 << endl;
	}
	else
	{
#define TEST_MODE(W, H, COLORMODEL)                                            \
	else if (                                                                  \
		options.frame_width == W && options.frame_height == H &&               \
		options.color_coding == COLOR_CODING_##COLORMODEL) m_desired_mode =    \
		DC1394_VIDEO_MODE_##W##x##H##_##COLORMODEL;

		if (false) {}
		TEST_MODE(160, 120, YUV444)
		TEST_MODE(320, 240, YUV422)
		TEST_MODE(640, 480, YUV411)
		TEST_MODE(640, 480, YUV422)
		TEST_MODE(640, 480, RGB8)
		TEST_MODE(640, 480, MONO8)
		TEST_MODE(640, 480, MONO16)
		TEST_MODE(800, 600, YUV422)
		TEST_MODE(800, 600, RGB8)
		TEST_MODE(800, 600, MONO8)
		TEST_MODE(800, 600, MONO16)
		TEST_MODE(1024, 768, YUV422)
		TEST_MODE(1024, 768, RGB8)
		TEST_MODE(1024, 768, MONO8)
		TEST_MODE(1024, 768, MONO16)
		TEST_MODE(1280, 960, YUV422)
		TEST_MODE(1280, 960, RGB8)
		TEST_MODE(1280, 960, MONO8)
		TEST_MODE(1280, 960, MONO16)
		TEST_MODE(1600, 1200, YUV422)
		TEST_MODE(1600, 1200, RGB8)
		TEST_MODE(1600, 1200, MONO8)
		TEST_MODE(1600, 1200, MONO16)
	}
	// Display all supported modes and chosen:
	if (verbose) cout << "------ Supported video modes ------" << endl;
	bool valid_video_mode = false;
	for (uint32_t i = 0; i < modes.num; i++)
	{
		string mode;
		switch (modes.modes[i])
		{
			case DC1394_VIDEO_MODE_160x120_YUV444:
				mode = "160x120_YUV444";
				break;
			case DC1394_VIDEO_MODE_320x240_YUV422:
				mode = "320x240_YUV422";
				break;
			case DC1394_VIDEO_MODE_640x480_YUV411:
				mode = "640x480_YUV411";
				break;
			case DC1394_VIDEO_MODE_640x480_YUV422:
				mode = "640x480_YUV422";
				break;
			case DC1394_VIDEO_MODE_640x480_RGB8: mode = "640x480_RGB8"; break;
			case DC1394_VIDEO_MODE_640x480_MONO8: mode = "640x480_MONO8"; break;
			case DC1394_VIDEO_MODE_640x480_MONO16:
				mode = "640x480_MONO16";
				break;
			case DC1394_VIDEO_MODE_800x600_YUV422:
				mode = "800x600_YUV422";
				break;
			case DC1394_VIDEO_MODE_800x600_RGB8: mode = "800x600_RGB8"; break;
			case DC1394_VIDEO_MODE_800x600_MONO8: mode = "800x600_MONO8"; break;
			case DC1394_VIDEO_MODE_1024x768_YUV422:
				mode = "1024x768_YUV422";
				break;
			case DC1394_VIDEO_MODE_1024x768_RGB8: mode = "1024x768_RGB8"; break;
			case DC1394_VIDEO_MODE_1024x768_MONO8:
				mode = "1024x768_MONO8";
				break;
			case DC1394_VIDEO_MODE_800x600_MONO16:
				mode = "800x600_MONO16";
				break;
			case DC1394_VIDEO_MODE_1024x768_MONO16:
				mode = "1024x768_MONO16";
				break;
			case DC1394_VIDEO_MODE_1280x960_YUV422:
				mode = "1280x960_YUV422";
				break;
			case DC1394_VIDEO_MODE_1280x960_RGB8: mode = "1280x960_RGB8"; break;
			case DC1394_VIDEO_MODE_1280x960_MONO8:
				mode = "1280x960_MONO8";
				break;
			case DC1394_VIDEO_MODE_1600x1200_YUV422:
				mode = "1600x1200_YUV422";
				break;
			case DC1394_VIDEO_MODE_1600x1200_RGB8:
				mode = "1600x1200_RGB8";
				break;
			case DC1394_VIDEO_MODE_1600x1200_MONO8:
				mode = "1600x1200_MONO8";
				break;
			case DC1394_VIDEO_MODE_1280x960_MONO16:
				mode = "1280x960_MONO16";
				break;
			case DC1394_VIDEO_MODE_1600x1200_MONO16:
				mode = "1600x1200_MONO16";
				break;
			case DC1394_VIDEO_MODE_EXIF: mode = "EXIF"; break;
			case DC1394_VIDEO_MODE_FORMAT7_0: mode = "FORMAT7_0"; break;
			case DC1394_VIDEO_MODE_FORMAT7_1: mode = "FORMAT7_1"; break;
			case DC1394_VIDEO_MODE_FORMAT7_2: mode = "FORMAT7_2"; break;
			case DC1394_VIDEO_MODE_FORMAT7_3: mode = "FORMAT7_3"; break;
			case DC1394_VIDEO_MODE_FORMAT7_4: mode = "FORMAT7_4"; break;
			case DC1394_VIDEO_MODE_FORMAT7_5: mode = "FORMAT7_5"; break;
			case DC1394_VIDEO_MODE_FORMAT7_6: mode = "FORMAT7_6"; break;
			case DC1394_VIDEO_MODE_FORMAT7_7: mode = "FORMAT7_7"; break;
			default:
				cerr << "[CImageGrabber_dc1394] ERROR: Requested video mode is "
						"not valid."
					 << endl;
				return;
		}
		if (modes.modes[i] == m_desired_mode) valid_video_mode = true;
		if (verbose)
		{
			if (modes.modes[i] == m_desired_mode)
				cout << mode << " (*)" << endl;
			else
				cout << mode << endl;
		}
	}
	if (!valid_video_mode)
	{
		cerr << format(
					"[CImageGrabber_dc1394] ERROR: Requested mode %ix%i "
					"color_model:%i is not available for this camera.",
					options.frame_width, options.frame_height,
					int(options.color_coding))
			 << endl;
		return;
	}

	// Reset to bus just in case:
	// And only once in a program, at start up:
	//	static bool reset_1394bus = true;
	//	if (reset_1394bus)
	{
		//		reset_1394bus = false;
		//		dc1394_reset_bus(THE_CAMERA);
	}

	/*-----------------------------------------------------------------------
	 *  setup capture
	 *-----------------------------------------------------------------------*/
	const int SIZE_RING_BUFFER = options.ring_buffer_size;

	err = dc1394_video_set_iso_speed(THE_CAMERA, DC1394_ISO_SPEED_400);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not set iso speed."
			 << endl;
		return;
	}

	err = dc1394_video_set_mode(THE_CAMERA, dc1394video_mode_t(m_desired_mode));
	// This checking only assures that m_desired_mode is inside
	// dc1394video_mode_t enum range
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not set video mode."
			 << endl;
		return;
	}

	dc1394framerate_t the_framerate;
	switch (m_options.framerate)
	{
		case FRAMERATE_1_875: the_framerate = DC1394_FRAMERATE_1_875; break;
		case FRAMERATE_3_75: the_framerate = DC1394_FRAMERATE_3_75; break;
		case FRAMERATE_7_5: the_framerate = DC1394_FRAMERATE_7_5; break;
		case FRAMERATE_15: the_framerate = DC1394_FRAMERATE_15; break;
		case FRAMERATE_30: the_framerate = DC1394_FRAMERATE_30; break;
		case FRAMERATE_60: the_framerate = DC1394_FRAMERATE_60; break;
		case FRAMERATE_120: the_framerate = DC1394_FRAMERATE_120; break;
		case FRAMERATE_240: the_framerate = DC1394_FRAMERATE_240; break;

		default:
			cerr << "[CImageGrabber_dc1394] ERROR: Requested framerate is not "
					"valid."
				 << endl;
			return;
	}

	err = dc1394_video_set_framerate(THE_CAMERA, the_framerate);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not set framerate."
			 << endl;
		return;
	}

	err = dc1394_capture_setup(
		THE_CAMERA, SIZE_RING_BUFFER, DC1394_CAPTURE_FLAGS_DEFAULT);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not setup camera-\nmake "
				"sure that the video mode and framerate are\nsupported by your "
				"camera."
			 << endl;
		return;
	}

	cout << "------ Other options ------" << endl;
	uint32_t iso_chan;
	if ((err = dc1394_video_get_iso_channel(THE_CAMERA, &iso_chan)) ==
		DC1394_SUCCESS)
		if (verbose) cout << "ISO Channel: " << iso_chan << endl;

	dc1394speed_t iso_speed;
	if ((err = dc1394_video_get_iso_speed(THE_CAMERA, &iso_speed)) ==
		DC1394_SUCCESS)
		if (verbose) cout << "ISO Speed: " << iso_speed << endl;

// set trigger options:
#define SET_TRIGGER(opt, OPT, TYPE)                                            \
	if (options.trigger_##opt >= 0)                                            \
	{                                                                          \
		err = dc1394_external_trigger_set_##opt(                               \
			THE_CAMERA,                                                        \
			static_cast<dc1394trigger_##opt##_t>(                              \
				DC1394_TRIGGER_##TYPE##_MIN + options.trigger_##opt));         \
		DC1394_WRN(                                                            \
			err,                                                               \
			"[CImageGrabber_dc1394::changeCaptureOptions] Could not set "      \
			"trigger opt");                                                    \
	}
	SET_TRIGGER(mode, MODE, MODE)
	SET_TRIGGER(source, SOURCE, SOURCE)
	SET_TRIGGER(polarity, POLARITY, ACTIVE)
	if (options.trigger_power >= 0)
	{
		err = dc1394_external_trigger_set_power(
			THE_CAMERA, dc1394switch_t(options.trigger_power));
		DC1394_WRN(
			err,
			"[CImageGrabber_dc1394::changeCaptureOptions] Could not set "
			"trigger power");
	}
#undef SET_TRIGGER

	/*-----------------------------------------------------------------------
	 *  have the camera start sending us data
	 *-----------------------------------------------------------------------*/
	err = dc1394_video_set_transmission(THE_CAMERA, DC1394_ON);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not start camera iso "
				"transmission."
			 << endl;
		return;
	}

	// remember that we successfully initialized everything
	m_bInitialized = true;

	changeCaptureOptions(m_options);

	// Camera current features:
	if (verbose)
	{
		dc1394featureset_t features;
		if ((err = dc1394_feature_get_all(THE_CAMERA, &features)) ==
			DC1394_SUCCESS)
			dc1394_feature_print_all(&features, stdout);
	}

#else
	THROW_EXCEPTION(
		"[CImageGrabber_dc1394] ERROR: MRPT compiled with "
		"MRPT_HAS_LIBDC1394_2=0 !");
#endif
	MRPT_END
}

/*-------------------------------------------------------------
					Destructor
 -------------------------------------------------------------*/
CImageGrabber_dc1394::~CImageGrabber_dc1394()
{
#if MRPT_HAS_LIBDC1394_2
	m_bInitialized = false;
	if (THE_CAMERA)
	{
		dc1394_video_set_transmission(THE_CAMERA, DC1394_OFF);
		dc1394_capture_stop(THE_CAMERA);

		// Release BW:
		//		uint32_t val;
		//		if (dc1394_video_get_bandwidth_usage(THE_CAMERA, &val) ==
		// DC1394_SUCCESS)
		//			 dc1394_iso_release_bandwidth(THE_CAMERA, val);
		//		if ( dc1394_video_get_iso_channel(THE_CAMERA, &val) ==
		// DC1394_SUCCESS)
		//			 dc1394_iso_release_channel(THE_CAMERA, val);

		dc1394_camera_free(THE_CAMERA);
	}
	if (THE_CONTEXT) { dc1394_free(THE_CONTEXT); }
#endif
}

/*-------------------------------------------------------------
					get the image - MONO
 -------------------------------------------------------------*/
bool CImageGrabber_dc1394::getObservation(
	mrpt::obs::CObservationImage& out_observation)
{
	MRPT_START

	if (!m_bInitialized) return false;

#if MRPT_HAS_LIBDC1394_2
	dc1394video_frame_t* frame = nullptr;
	dc1394error_t err;

	err = dc1394_video_set_transmission(THE_CAMERA, DC1394_ON);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not start camera iso "
				"transmission."
			 << endl;
		return false;
	}

	// get frame from ring buffer:
	err =
		dc1394_capture_dequeue(THE_CAMERA, DC1394_CAPTURE_POLICY_WAIT, &frame);
	// dc1394error_t err=dc1394_capture_dequeue(THE_CAMERA,
	// DC1394_CAPTURE_POLICY_POLL, &frame);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not capture a frame"
			 << endl;
		return false;
	}

	out_observation.timestamp = mrpt::system::now();

	const unsigned int width = frame->size[0];
	const unsigned int height = frame->size[1];

	if (!m_options.deinterlace_stereo)
	{
		/*-----------------------------------------------------------------------
		 *  convert the image from what ever format it is to its RGB8
		 *-----------------------------------------------------------------------*/
		// dc1394_get_image_size_from_video_mode(THE_CAMERA, m_desired_mode,
		// &width, &height);

		auto* new_frame = static_cast<dc1394video_frame_t*>(
			calloc(1, sizeof(dc1394video_frame_t)));
		new_frame->color_coding = DC1394_COLOR_CODING_RGB8;
		dc1394_convert_frames(frame, new_frame);

		// Fill the output class:
		out_observation.image.loadFromMemoryBuffer(
			width, height, true, new_frame->image, true /* BGR -> RGB */);

		// Free temporary frame:
		free(new_frame->image);
		free(new_frame);
	}
	else
	{
		// Stereo images:
		auto* imageBuf = new uint8_t[width * height * 2];
		auto* imageBufRGB = new uint8_t[width * height * 2 * 3];

		if ((err = dc1394_deinterlace_stereo(
				 frame->image, imageBuf, width, 2 * height)) != DC1394_SUCCESS)
		{
			cerr << "[CImageGrabber_dc1394] ERROR: Could not deinterlace "
					"stereo images: "
				 << err << endl;
			return false;
		}

		if ((err = dc1394_bayer_decoding_8bit(
				 imageBuf, imageBufRGB, width, 2 * height,
				 DC1394_COLOR_FILTER_GBRG,	// Has to be this value for
				 // Bumblebee!
				 DC1394_BAYER_METHOD_HQLINEAR)) != DC1394_SUCCESS)
		{
			cerr << "[CImageGrabber_dc1394] ERROR: Could not apply Bayer "
					"conversion: "
				 << err << endl;
			return false;
		}

		out_observation.image.loadFromMemoryBuffer(
			width, height, true, imageBufRGB);	// Left cam.
		// out_observation.image.loadFromMemoryBuffer(width,height,true,
		// imageBufRGB+ width*height*3 ); // Right cam.

		delete[] imageBuf;
		delete[] imageBufRGB;
	}

	// Now we can return the frame to the ring buffer:
	err = dc1394_capture_enqueue(THE_CAMERA, frame);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not enqueue the ring "
				"buffer frame"
			 << endl;
		return false;
	}

	return true;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
#endif
	MRPT_END
}

/*-------------------------------------------------------------
					get the image - STEREO
 -------------------------------------------------------------*/
bool CImageGrabber_dc1394::getObservation(
	mrpt::obs::CObservationStereoImages& out_observation)
{
	MRPT_START

	if (!m_bInitialized) return false;

#if MRPT_HAS_LIBDC1394_2
	dc1394video_frame_t* frame = nullptr;

	// get frame from ring buffer:
	dc1394error_t err =
		dc1394_capture_dequeue(THE_CAMERA, DC1394_CAPTURE_POLICY_WAIT, &frame);
	if (err != DC1394_SUCCESS)
	{
		cerr << "[CImageGrabber_dc1394] ERROR: Could not capture a frame"
			 << endl;
		return false;
	}

	out_observation.timestamp = mrpt::system::now();

	const unsigned int width = frame->size[0];
	const unsigned int height = frame->size[1];

	if (!m_options.deinterlace_stereo)
	{
		THROW_EXCEPTION(
			"Call to getObservation(stereo) but the camera was not set as "
			"stereo!");
	}
	else
	{
		// Stereo images:
		auto* imageBuf = new uint8_t[width * height * 2];
		auto* imageBufRGB = new uint8_t[width * height * 2 * 3];

		if ((err = dc1394_deinterlace_stereo(
				 frame->image, imageBuf, width, 2 * height)) != DC1394_SUCCESS)
		{
			cerr << "[CImageGrabber_dc1394] ERROR: Could not deinterlace "
					"stereo images: "
				 << err << endl;
			return false;
		}

		if ((err = dc1394_bayer_decoding_8bit(
				 imageBuf, imageBufRGB, width, 2 * height,
				 DC1394_COLOR_FILTER_GBRG,	// Has to be this value for
				 // Bumblebee!
				 DC1394_BAYER_METHOD_HQLINEAR)) != DC1394_SUCCESS)
		{
			cerr << "[CImageGrabber_dc1394] ERROR: Could not apply Bayer "
					"conversion: "
				 << err << endl;
			return false;
		}

		out_observation.imageLeft.loadFromMemoryBuffer(
			width, height, true, imageBufRGB);	// Left cam.
		out_observation.imageRight.loadFromMemoryBuffer(
			width, height, true,
			imageBufRGB + width * height * 3);	// Right cam.

		delete[] imageBuf;
		delete[] imageBufRGB;
	}

	// Now we can return the frame to the ring buffer:
	dc1394_capture_enqueue(THE_CAMERA, frame);

	return true;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
#endif
	MRPT_END
}

/*-------------------------------------------------------------
					changeCaptureOptions
 -------------------------------------------------------------*/
bool CImageGrabber_dc1394::changeCaptureOptions(
	const TCaptureOptions_dc1394& options)
{
	MRPT_START

	if (!m_bInitialized) return false;

#if MRPT_HAS_LIBDC1394_2
	dc1394error_t err;
// set features modes:
#define SET_MODE(feat, FEAT)                                                   \
	if (options.feat##_mode >= 0)                                              \
	{                                                                          \
		err = dc1394_feature_set_mode(                                         \
			THE_CAMERA, DC1394_FEATURE_##FEAT,                                 \
			static_cast<dc1394feature_mode_t>(                                 \
				DC1394_FEATURE_MODE_MIN + options.feat##_mode));               \
		DC1394_WRN(                                                            \
			err,                                                               \
			"[CImageGrabber_dc1394::changeCaptureOptions] Could not set feat " \
			"mode");                                                           \
	}
	SET_MODE(shutter, SHUTTER)
	SET_MODE(gain, GAIN)
	SET_MODE(gamma, GAMMA)
	SET_MODE(brightness, BRIGHTNESS)
	SET_MODE(exposure, EXPOSURE)
	SET_MODE(sharpness, SHARPNESS)
	SET_MODE(white_balance, WHITE_BALANCE)
#undef SET_MODE

// Set features values:
#define SET_VALUE(feat, FEAT)                                                  \
	if (options.feat >= 0)                                                     \
	{                                                                          \
		err = dc1394_feature_set_value(                                        \
			THE_CAMERA, DC1394_FEATURE_##FEAT, options.feat);                  \
		DC1394_WRN(                                                            \
			err,                                                               \
			"[CImageGrabber_dc1394::changeCaptureOptions] Could not set feat " \
			"value");                                                          \
	}
	SET_VALUE(shutter, SHUTTER)
	SET_VALUE(gain, GAIN)
	SET_VALUE(gamma, GAMMA)
	SET_VALUE(brightness, BRIGHTNESS)
	SET_VALUE(exposure, EXPOSURE)
	SET_VALUE(sharpness, SHARPNESS)
	SET_VALUE(white_balance, WHITE_BALANCE)
#undef SET_VALUE

	return true;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
#endif
	MRPT_END
}

/*-------------------------------------------------------------
					setSoftwareTriggerLevel
 -------------------------------------------------------------*/
bool CImageGrabber_dc1394::setSoftwareTriggerLevel(bool level)
{
	MRPT_START

	if (!m_bInitialized) return false;

#if MRPT_HAS_LIBDC1394_2
	dc1394error_t err;
	err = dc1394_software_trigger_set_power(THE_CAMERA, (dc1394switch_t)level);
	DC1394_WRN(
		err,
		"[CImageGrabber_dc1394::setSoftwareTriggerLevel] Could not set "
		"software trigger level");

	return true;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
#endif
	MRPT_END
}

/** Generates a list with the information on all the existing (Firewire) cameras
 * in the system.
 * \exception std::runtime_error On any error calling libdc1394.
 */
void CImageGrabber_dc1394::enumerateCameras(TCameraInfoList& out_list)
{
	MRPT_START
#if MRPT_HAS_LIBDC1394_2

	dc1394_t* lib_context = nullptr;
	dc1394camera_list_t* list = nullptr;
	out_list.clear();

	try
	{
		lib_context = dc1394_new();
		if (!lib_context)
			throw std::runtime_error(
				"[CImageGrabber_dc1394] ERROR: Failed to enumerate cameras "
				"(Maybe your user has no rights to access IEEE1394?).");

		// Enumerate cameras:
		dc1394error_t err;

		err = dc1394_camera_enumerate(lib_context, &list);
		if (err != DC1394_SUCCESS)
			throw std::runtime_error(
				"[CImageGrabber_dc1394] ERROR: Failed to enumerate cameras "
				"(Maybe your user has no rights to access IEEE1394?).");

		for (unsigned int i = 0; i < list->num; i++)
		{
			TCameraInfo info;

			info.guid = list->ids[i].guid;
			info.unit = list->ids[i].unit;

			// Try to open it:
			dc1394camera_t* cam = dc1394_camera_new_unit(
				lib_context, list->ids[i].guid, list->ids[i].unit);
			if (!cam)
				throw std::runtime_error(format(
					"[CImageGrabber_dc1394] ERROR: Failed to query camera "
					"with GUID %u\n",
					static_cast<unsigned int>(list->ids[i].guid)));

			info.unit_spec_ID = cam->unit_spec_ID;
			info.unit_sw_version = cam->unit_sw_version;
			info.unit_sub_sw_version = cam->unit_sub_sw_version;
			info.command_registers_base = cam->command_registers_base;
			info.unit_directory = cam->unit_directory;
			info.unit_dependent_directory = cam->unit_dependent_directory;
			info.advanced_features_csr = cam->advanced_features_csr;
			info.PIO_control_csr = cam->PIO_control_csr;
			info.SIO_control_csr = cam->SIO_control_csr;
			info.strobe_control_csr = cam->strobe_control_csr;
			for (int j = 0; j < DC1394_VIDEO_MODE_FORMAT7_NUM; j++)
				info.format7_csr[j] = cam->format7_csr[j];
			info.iidc_version = cam->iidc_version;
			info.vendor = std::string(cam->vendor ? cam->vendor : "");
			info.model = std::string(cam->model ? cam->model : "");
			info.vendor_id = cam->vendor_id;
			info.model_id = cam->model_id;
			info.bmode_capable = cam->bmode_capable;
			info.one_shot_capable = cam->one_shot_capable;
			info.multi_shot_capable = cam->multi_shot_capable;
			info.can_switch_on_off = cam->can_switch_on_off;
			info.has_vmode_error_status = cam->has_vmode_error_status;
			info.has_feature_error_status = cam->has_feature_error_status;
			info.max_mem_channel = cam->max_mem_channel;

			// dc1394_camera_print_info(cam,stdout);

			dc1394_camera_free(cam);  // Close camera

			out_list.push_back(info);
		}

		// Free context:
		dc1394_free(lib_context);
		lib_context = nullptr;
		dc1394_camera_free_list(list);
		list = nullptr;
	}
	catch (...)
	{
		if (list) dc1394_camera_free_list(list);
		if (lib_context) dc1394_free(lib_context);
		throw;
	}
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_LIBDC1394_2=0 !");
#endif
	MRPT_END
}
