/* +------------------------------------------------------------------------+
   |                     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/3rdparty/do_opencv_includes.h>
#include <mrpt/hwdrivers/CImageGrabber_OpenCV.h>

#include <chrono>
#include <thread>

#ifdef HAVE_OPENCV_VIDEOIO
// cv::VideoCapture moved from highgui in opencv2 to videoio in opencv3:
#include <opencv2/videoio.hpp>
#endif

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

struct CImageGrabber_OpenCV::Impl
{
#if MRPT_HAS_OPENCV
	cv::VideoCapture cap;
#endif
};

/*-------------------------------------------------------------
				Constructor
-------------------------------------------------------------*/
CImageGrabber_OpenCV::CImageGrabber_OpenCV(
	int cameraIndex, TCameraType cameraType, const TCaptureCVOptions& options)
	: m_capture(mrpt::make_impl<CImageGrabber_OpenCV::Impl>())
{
	MRPT_START
	m_bInitialized = false;

#if MRPT_HAS_OPENCV
	int cv_cap_indx = 0;
	switch (cameraType)
	{
		case CAMERA_CV_AUTODETECT: cv_cap_indx = CV_CAP_ANY; break;
		case CAMERA_CV_DC1394: cv_cap_indx = CV_CAP_DC1394; break;
		case CAMERA_CV_VFL: cv_cap_indx = CV_CAP_V4L; break;
		case CAMERA_CV_VFW: cv_cap_indx = CV_CAP_VFW; break;
		case CAMERA_CV_MIL: cv_cap_indx = CV_CAP_MIL; break;
		case CAMERA_CV_DSHOW: cv_cap_indx = CV_CAP_DSHOW; break;
		default: THROW_EXCEPTION_FMT("Invalid camera type: %i", cameraType);
	}

	cv_cap_indx += cameraIndex;

	// Open camera:
	if (!m_capture->cap.open(cv_cap_indx))
	{
		cerr << format(
			"[CImageGrabber_OpenCV] ERROR: Can't open camera '%i'!!\n",
			cameraIndex);
		return;
	}

	// Set properties:
	// Based on code from Orocos project. Thanks!
	// ----------------------------------------
	// Global settings
	if (options.gain != 0)
	{
		if (!m_capture->cap.set(CV_CAP_PROP_GAIN, options.gain))
			cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
					"capturing gain property!"
				 << endl;
	}

	// Settings only for firewire
	if (cameraType == CAMERA_CV_DC1394)
	{
		if (options.frame_height != 0 && options.frame_width != 0)
		{
			// MODE_320x240_YUV422 ****
			//
			enum
			{
				MY_MODE_160x120_YUV444 = 64,
				MY_MODE_320x240_YUV422,	 // ***
				MY_MODE_640x480_YUV411,
				MY_MODE_640x480_YUV422,	 // ***
				MY_MODE_640x480_RGB,  // ?
				MY_MODE_640x480_MONO,  // ***
				MY_MODE_640x480_MONO16
			};

			int cvMode1394 = -1;
			if (options.frame_height == 320 && options.frame_width == 240)
				cvMode1394 = MY_MODE_320x240_YUV422;
			else if (
				options.frame_height == 640 && options.frame_width == 480 &&
				!options.ieee1394_grayscale)
				cvMode1394 = MY_MODE_640x480_YUV422;
			else if (
				options.frame_height == 640 && options.frame_width == 480 &&
				options.ieee1394_grayscale)
				cvMode1394 = MY_MODE_640x480_MONO;

			if (cvMode1394 > 0)
			{
				if (!m_capture->cap.set(CV_CAP_PROP_MODE, cvMode1394))
					cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
							"capturing mode "
						 << cvMode1394 << " property!" << endl;
			}
			else
				cerr << "[CImageGrabber_OpenCV] Warning: Not valid combination "
						"of width x height x color mode for OpenCV/IEEE1394 "
						"interface"
					 << endl;
		}

		// Not needed: Default seems to be = 1
		// if(cvSetCaptureProperty(M_CAPTURE,CV_CAP_PROP_CONVERT_RGB,_capture_convert.value())<1)
		//	cerr << "[CImageGrabber_OpenCV] Warning: Could not set the RGB
		// conversion property!" << endl;

		if (!m_capture->cap.set(CV_CAP_PROP_FPS, options.ieee1394_fps))
			cerr << "[CImageGrabber_OpenCV] Warning: Could not set the fps "
					"property!"
				 << endl;
	}

	// Settings only for V4L
	if (cameraType == CAMERA_CV_AUTODETECT || cameraType == CAMERA_CV_VFL ||
		cameraType == CAMERA_CV_VFW || cameraType == CAMERA_CV_DSHOW)
	{
		if (options.frame_width != 0 && options.frame_height != 0)
		{
			// First set width then height. The first command always returns a
			// error!
			m_capture->cap.set(CV_CAP_PROP_FRAME_WIDTH, options.frame_width);
			if (!m_capture->cap.set(
					CV_CAP_PROP_FRAME_HEIGHT, options.frame_height))
				cerr << "[CImageGrabber_OpenCV] Warning: Could not set the "
						"frame width & height property!"
					 << endl;
		}
	}

	// remember that we successfully initialized everything
	m_bInitialized = true;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif
	MRPT_END
}

CImageGrabber_OpenCV::CImageGrabber_OpenCV(const std::string& AVI_fileName)
	: m_capture(mrpt::make_impl<CImageGrabber_OpenCV::Impl>())
{
	MRPT_START
	m_bInitialized = false;

#if MRPT_HAS_OPENCV
	if (!m_capture->cap.open(AVI_fileName))
	{
		printf(
			"[CImageGrabber_OpenCV] Warning! Can't open AVI file '%s'!!\n",
			AVI_fileName.c_str());
		return;
	}

	// remember that we successfully initialized everything
	m_bInitialized = true;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif
	MRPT_END
}

/*-------------------------------------------------------------
					Destructor
 -------------------------------------------------------------*/
CImageGrabber_OpenCV::~CImageGrabber_OpenCV()
{
#if MRPT_HAS_OPENCV
	if (m_bInitialized) { m_capture->cap.release(); }
#endif
}

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

	if (!m_bInitialized) return false;

#if MRPT_HAS_OPENCV

	// Capture the image:
	if (!m_capture->cap.grab()) return false;

	// JL: Sometimes there're errors in some frames: try not to return an error
	// unless it seems
	//  there's no way:
	for (int nTries = 0; nTries < 10; nTries++)
	{
		cv::Mat capImg;
		if (m_capture->cap.retrieve(capImg))
		{
			// Fill the output class:
			out_observation.timestamp = mrpt::system::now();
			out_observation.image =
				mrpt::img::CImage(capImg, mrpt::img::SHALLOW_COPY);
			return true;
		}
		cerr << "[CImageGrabber_OpenCV] WARNING: Ignoring error #" << nTries + 1
			 << " retrieving frame..." << endl;
		std::this_thread::sleep_for(1ms);
	}
	return false;
#else
	THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif
	MRPT_END
}
