/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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 "obs-precomp.h"  // Precompiled headers
//
#include <mrpt/containers/yaml.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/serialization/CArchive.h>

#if MRPT_HAS_MATLAB
#include <mexplus/mxarray.h>
#endif
#include <algorithm>  // all_of

using namespace mrpt::obs;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::img;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservationStereoImages, CObservation, mrpt::obs)

uint8_t CObservationStereoImages::serializeGetVersion() const { return 6; }
void CObservationStereoImages::serializeTo(
	mrpt::serialization::CArchive& out) const
{
	// The data
	out << cameraPose << leftCamera << rightCamera << imageLeft;
	out << hasImageDisparity << hasImageRight;
	if (hasImageRight) out << imageRight;
	if (hasImageDisparity) out << imageDisparity;
	out << timestamp;
	out << rightCameraPose;
	out << sensorLabel;
}

void CObservationStereoImages::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 6:
		{
			in >> cameraPose >> leftCamera >> rightCamera >> imageLeft;
			in >> hasImageDisparity >> hasImageRight;
			if (hasImageRight) in >> imageRight;
			if (hasImageDisparity) in >> imageDisparity;
			in >> timestamp;
			in >> rightCameraPose;
			in >> sensorLabel;
		}
		break;

		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
		case 5:
		{
			// This, for backwards compatibility before version 6:
			hasImageRight = true;
			hasImageDisparity = false;

			if (version < 5)
			{
				CPose3D aux;
				in >> aux;
				cameraPose = CPose3DQuat(aux);
			}

			if (version >= 5) { in >> cameraPose >> leftCamera >> rightCamera; }
			else
			{
				CMatrixF intParams;
				// Get the intrinsic params
				in >> intParams;
				// Set them to both cameras
				leftCamera.intrinsicParams = intParams;
				// ... distortion parameters are set to zero
				rightCamera.intrinsicParams = intParams;
			}

			in >> imageLeft >> imageRight;	// For all the versions

			if (version >= 1) in >> timestamp;
			else
				timestamp = INVALID_TIMESTAMP;	// For version 1 to 5
			if (version >= 2)
			{
				if (version < 5)
				{
					CPose3D aux;
					in >> aux;
					rightCameraPose = CPose3DQuat(aux);
				}
				else
					in >> rightCameraPose;
			}
			else
				rightCameraPose = CPose3DQuat(
					0.10f, 0, 0,
					mrpt::math::CQuaternionDouble(
						1, 0, 0, 0));  // For version 1 to 5

			if (version >= 3 && version < 5)  // For versions 3 & 4
			{
				double foc;
				in >> foc;	// Get the focal length in meters
				leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
					foc;  // ... and set it to both cameras
			}
			else if (version < 3)
				leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
					0.002;	// For version 0, 1 & 2 (from version 5, this
			// parameter is included in the TCamera objects)

			if (version >= 4) in >> sensorLabel;
			else
				sensorLabel = "";  // For version 1 to 5
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::obs::CObservationStereoImages)
#endif

mxArray* CObservationStereoImages::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
	const char* fields[] = {"class",   "ts",	 "sensorLabel", "imageL",
							"imageR",  "poseL",	 "poseLR",		"poseR",
							"paramsL", "paramsR"};
	mexplus::MxArray obs_struct(
		mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));

	obs_struct.set("class", this->GetRuntimeClass()->className);
	obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
	obs_struct.set("sensorLabel", this->sensorLabel);
	obs_struct.set("imageL", this->imageLeft);
	obs_struct.set("imageR", this->imageRight);
	obs_struct.set("poseL", this->cameraPose);
	obs_struct.set("poseR", this->cameraPose + this->rightCameraPose);
	obs_struct.set("poseLR", this->rightCameraPose);
	obs_struct.set("paramsL", this->leftCamera);
	obs_struct.set("paramsR", this->rightCamera);
	return obs_struct.release();
#else
	THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
#endif
}

/** Populates a TStereoCamera structure with the parameters in \a leftCamera, \a
 * rightCamera and \a rightCameraPose */
void CObservationStereoImages::getStereoCameraParams(
	mrpt::img::TStereoCamera& out_params) const
{
	out_params.leftCamera = this->leftCamera;
	out_params.rightCamera = this->rightCamera;
	out_params.rightCameraPose = this->rightCameraPose.asTPose();
}

/** Sets \a leftCamera, \a rightCamera and \a rightCameraPose from a
 * TStereoCamera structure */
void CObservationStereoImages::setStereoCameraParams(
	const mrpt::img::TStereoCamera& in_params)
{
	this->leftCamera = in_params.leftCamera;
	this->rightCamera = in_params.rightCamera;
	this->rightCameraPose = in_params.rightCameraPose;
}

/** This method only checks whether ALL the distortion parameters in \a
 * leftCamera are set to zero, which is
 * the convention in MRPT to denote that this pair of stereo images has been
 * rectified.
 */
bool CObservationStereoImages::areImagesRectified() const
{
	return std::all_of(
		leftCamera.dist.begin(), leftCamera.dist.end(),
		[](auto v) { return v == 0; });
}

// Do an efficient swap of all data members of this object with "o".
void CObservationStereoImages::swap(CObservationStereoImages& o)
{
	CObservation::swap(o);

	imageLeft.swap(o.imageLeft);
	imageRight.swap(o.imageRight);
	imageDisparity.swap(o.imageDisparity);

	std::swap(hasImageDisparity, o.hasImageDisparity);
	std::swap(hasImageRight, o.hasImageRight);

	std::swap(leftCamera, o.leftCamera);
	std::swap(rightCamera, o.rightCamera);

	std::swap(cameraPose, o.cameraPose);
	std::swap(rightCameraPose, o.rightCameraPose);
}

void CObservationStereoImages::getDescriptionAsText(std::ostream& o) const
{
	using namespace std;
	CObservation::getDescriptionAsText(o);

	o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
		 "base:\n";
	o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << "\n"
	  << "Camera pose: " << cameraPose << "\n"
	  << "Camera pose (YPR): " << CPose3D(cameraPose) << "\n"
	  << "\n";

	const mrpt::img::TStereoCamera stParams = getStereoCameraParams();
	o << stParams.dumpAsText() << "\n";

	o << "Right camera pose wrt left camera (YPR):"
	  << "\n"
	  << CPose3D(stParams.rightCameraPose) << "\n";

	if (imageLeft.isExternallyStored())
		o << " Left image is stored externally in file: "
		  << imageLeft.getExternalStorageFile() << "\n";

	o << " Right image";
	if (hasImageRight)
	{
		if (imageRight.isExternallyStored())
			o << " is stored externally in file: "
			  << imageRight.getExternalStorageFile() << "\n";
	}
	else
		o << " : No.\n";

	o << " Disparity image";
	if (hasImageDisparity)
	{
		if (imageDisparity.isExternallyStored())
			o << " is stored externally in file: "
			  << imageDisparity.getExternalStorageFile() << "\n";
	}
	else
		o << " : No.\n";

	if (!imageLeft.isEmpty())
	{
		o << format(
			" Image size: %ux%u pixels\n", (unsigned int)imageLeft.getWidth(),
			(unsigned int)imageLeft.getHeight());

		o << " Channels order: " << imageLeft.getChannelsOrder() << "\n";

		o << format(
			" Rows are stored in top-bottom order: %s\n",
			imageLeft.isOriginTopLeft() ? "YES" : "NO");
	}

	o << "\n# Left camera calibration:\n" << stParams.leftCamera.asYAML();
	o << "\n# Right camera calibration:\n" << stParams.rightCamera.asYAML();
}

void CObservationStereoImages::load() const
{
	imageLeft.forceLoad();
	imageRight.forceLoad();
	imageDisparity.forceLoad();
}
