/* +------------------------------------------------------------------------+
   |                     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/hwdrivers/CStereoGrabber_SVS.h>

// Universal include for all versions of OpenCV
#include <mrpt/3rdparty/do_opencv_includes.h>

#if MRPT_HAS_SVS
#include <dcs.h>
#include <svsclass.h>
#endif

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

/*-------------------------------------------------------------
					Constructor
 -------------------------------------------------------------*/
CStereoGrabber_SVS::CStereoGrabber_SVS(
	[[maybe_unused]] int cameraIndex,
	[[maybe_unused]] const TCaptureOptions_SVS& options)
	: m_resolutionX(options.frame_width),
	  m_resolutionY(options.frame_height),
	  m_procesOnChip(options.m_procesOnChip),
	  m_calDisparity(options.m_calDisparity),
	  m_options(options)
{
#if MRPT_HAS_SVS

	// get the svsVideoImages object from the currently loaded camera interface
	m_videoObject = static_cast<svsVideoImages*>(getVideoObject());
	cout << "Using live images:" << endl;
	cout << "svsVideoIdent" << endl;

	// Open the stereo device
	bool ret;
	ret = static_cast<svsVideoImages*>(m_videoObject)->Open();
	if (ret)
	{
		cout << " stereo device Opened" << endl;

		static_cast<svsVideoImages*>(m_videoObject)
			->SetSize(m_resolutionX, m_resolutionY);  // width x height image

		m_disparityParams =
			static_cast<svsVideoImages*>(m_videoObject)->GetDP();
		static_cast<svsVideoImages*>(m_videoObject)
			->SetNDisp(m_options.m_NDisp);	// 32 disparities
		static_cast<svsVideoImages*>(m_videoObject)
			->SetCorrsize(m_options.m_Corrsize);  // correlation window size
		static_cast<svsVideoImages*>(m_videoObject)
			->SetLR(m_options.m_LR);  // no left-right check, not available
		static_cast<svsVideoImages*>(m_videoObject)
			->SetThresh(m_options.m_Thresh);  // texture filter
		static_cast<svsVideoImages*>(m_videoObject)
			->SetUnique(m_options.m_Unique);  // uniqueness filter
		static_cast<svsVideoImages*>(m_videoObject)
			->SetHoropter(m_options.m_Horopter);  // horopter offset

		if (!(static_cast<svsVideoImages*>(m_videoObject)
				  ->SetExposure(0, 0, true, true)))
		{
			cout << "Can't set Auto exposure" << endl;
		}
		else
		{
			cout << "Autoexposure set to 0 0" << endl;
		}

		/*	videoObject->SetBrightness(true, 0);
		   videoObject->SetAutoExpParams(0.0, -2.0);
	   */
		/// videoObject->SetGamma(); search for auto gamma ?

		static_cast<svsVideoImages*>(m_videoObject)
			->SetRate(m_options.framerate);
		static_cast<svsVideoImages*>(m_videoObject)
			->SetSpeckleSize(m_options.m_SpeckleSize);	// TODO add in config

		// TODO call CheckParam
		if (static_cast<svsVideoImages*>(m_videoObject)->CheckParams())
		{
			cout << "Params OK !" << endl;
			m_initialized = true;
			m_status = true;
			bool ret = static_cast<svsVideoImages*>(m_videoObject)->Start();
			if (ret)
			{
				cout << " Start Continuous mode" << endl;

				// NOTE: to do rectification, we have to turn it on...
				// Here we optionally set up acquisition to rectify the image

				ret =
					static_cast<svsVideoImages*>(m_videoObject)->SetRect(true);
				if (ret) { cout << "Images will be rectified" << endl; }
				else
				{
					cout << "Can't set rectification" << endl;
				}
				if (m_procesOnChip)
				{
					// NOTE: for STOC device, turn on stereo processing on-chip
					if (static_cast<svsVideoImages*>(m_videoObject) &&
						static_cast<svsVideoImages*>(m_videoObject)
							->is_proc_capable)	// can we process on-camera?
					{
						static_cast<svsVideoImages*>(m_videoObject)
							->SetProcMode(PROC_MODE_DISPARITY);
						cout << "Setting STOC disparity mode" << endl;
					}
				}
				else
				{
					if (m_processObject)
						m_processObject = new svsStereoProcess();
					// NOTE: for STOC device, turn off stereo processing on-chip
					if (static_cast<svsVideoImages*>(m_videoObject) &&
						static_cast<svsVideoImages*>(m_videoObject)
							->is_proc_capable)	// can we process on-camera?
					{
						static_cast<svsVideoImages*>(m_videoObject)
							->SetProcMode(PROC_MODE_OFF);
						cout << "Setting STOC stereo mode" << endl;
					}
				}
			}
			else
			{
				cout << "Can't start continuous capture" << endl;
				m_status = false;
			}
		}
		else
		{
			m_initialized = false;
			m_status = false;
			cout << "Params Unconsistents !" << endl;
		}
	}
	else

		// TODO essayer de faire un close...
		cout << "Can't open stereo device" << endl;

	m_status = false;

#else
	THROW_EXCEPTION("This class requires MRPT built with Videre SVS library.");
#endif
}

/*-------------------------------------------------------------
					Destructor
 -------------------------------------------------------------*/
CStereoGrabber_SVS::~CStereoGrabber_SVS()
{
#if MRPT_HAS_SVS
	static_cast<svsVideoImages*>(m_videoObject)->Close();
#endif	// No need to raise an exception on "#else" since it's already raised
	// upon construction.
}

/*-------------------------------------------------------------
					get the image
 -------------------------------------------------------------*/
bool CStereoGrabber_SVS::getStereoObservation(
	[[maybe_unused]] mrpt::obs::CObservationStereoImages& out_observation)
{
#if MRPT_HAS_SVS
	if ((m_stereoImage =
			 static_cast<svsVideoImages*>(m_videoObject)->GetImage(500)) &&
		static_cast<svsStereoImage*>(m_stereoImage)->haveImages)  // 500 ms
	// timeout
	// //TODO
	// adjust
	// timeout
	// with
	// framerate
	{
		// get disparity params
		m_disparityParams =
			static_cast<svsVideoImages*>(m_videoObject)->GetDP();

		const size_t sizeOfMat = m_resolutionX * m_resolutionY;

		IplImage* ImageLeft = cvCreateImageHeader(
			cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);

		ImageLeft->widthStep =
			ImageLeft->width;  // JL: The next line assumes this
		ImageLeft->imageData =
			(char*)static_cast<svsStereoImage*>(m_stereoImage)->Left();

		if (m_procesOnChip)
		{
			IplImage* ImageDisparity = cvCreateImage(
				cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);

			unsigned char* ptrOutDisp;
			short int* ptrDisp;

			ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
			ptrOutDisp = (unsigned char*)ImageDisparity->imageData;

			ASSERT_(
				ImageDisparity->widthStep ==
				ImageDisparity->width);	 // JL: The code below assumes
			// image_width == widthStep

			for (int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
			{
				if (*(ptrDisp) > 0)
					*(ptrOutDisp) = (unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
				else
					*(ptrOutDisp) = 0;
			}

			// Create the object to be return (it will have a fresh timestamp if
			// it's created now and here):
			CObservationStereoImages ret_obj(
				cvCloneImage(ImageLeft),  // Create a new IplImage* which will
				// be owned by the observation object.
				nullptr /*has no right*/, ImageDisparity,
				true /* own the memory, so we don't have to free it here */);

			out_observation.swap(
				ret_obj);  // Send as output (faster than a "=").
		}
		else if (m_calDisparity)
		{
			static_cast<svsStereoProcess*>(m_processObject)
				->CalcStereo(static_cast<svsStereoImage*>(m_stereoImage));
			IplImage* ImageDisparity = cvCreateImage(
				cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);

			unsigned char* ptrOutDisp;
			short int* ptrDisp;

			ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
			ptrOutDisp = (unsigned char*)ImageDisparity->imageData;

			ASSERT_(
				ImageDisparity->widthStep ==
				ImageDisparity->width);	 // JL: The code below assumes
			// image_width == widthStep

			for (int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
			{
				if (*(ptrDisp) > 0)
					*(ptrOutDisp) = (unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
				else
					*(ptrOutDisp) = 0;
			}

			// Create the object to be return (it will have a fresh timestamp if
			// it's created now and here):
			CObservationStereoImages ret_obj(
				cvCloneImage(ImageLeft),  // Create a new IplImage* which will
				// be owned by the observation object.
				nullptr /*has no right*/, ImageDisparity,
				true /* own the memory, so we don't have to free it here */);

			out_observation.swap(
				ret_obj);  // Send as output (faster than a "=").
		}
		else
		{
			IplImage* ImageRight = cvCreateImageHeader(
				cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);
			ImageRight->widthStep =
				ImageRight->width;	// JL: The next line assumes this
			ImageRight->imageData =
				(char*)static_cast<svsStereoImage*>(m_stereoImage)->Right();

			// Create the object to be return (it will have a fresh timestamp if
			// it's created now and here):
			CObservationStereoImages ret_obj(
				cvCloneImage(ImageLeft),  // Create a new IplImage* which will
				// be owned by the observation object.
				cvCloneImage(ImageRight), nullptr /*has no disparity*/,
				true /* own the memory, so we don't have to free it here */);

			out_observation.swap(
				ret_obj);  // Send as output (faster than a "=").
		}

		// cvReleaseImage(&Image Disparity); // No need anymore to release
		// images...
		return true;
	}

	return false;
// All ok
#else
	// No need to raise an exception on "#else" since it's already raised upon
	// construction.
	return false;  // This shouldn't actually be never reached, just to please
// the compiler.
#endif
}

TCaptureOptions_SVS::TCaptureOptions_SVS(
	int _frame_width, int _frame_height, double _framerate, int _NDisp,
	int _Corrsize, int _LR, int _Thresh, int _Unique, int _Horopter,
	int _SpeckleSize, bool _procesOnChip, bool _calDisparity)
{
	frame_width = _frame_width;
	frame_height = _frame_height;
	framerate = _framerate;
	m_NDisp = _NDisp;  // 32 disparities
	m_Corrsize = _Corrsize;	 // correlation window size
	m_LR = _LR;	 // no left-right check, not available
	m_Thresh = _Thresh;	 // texture filter
	m_Unique = _Unique;	 // uniqueness filter
	m_Horopter = _Horopter;
	m_SpeckleSize = _SpeckleSize;
	m_procesOnChip = _procesOnChip;
	m_calDisparity = _calDisparity;
}
