/* +------------------------------------------------------------------------+
   |                     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 "img-precomp.h"  // Precompiled headers
//
#include <mrpt/core/round.h>
#include <mrpt/img/CMappedImage.h>

using namespace mrpt;
using namespace mrpt::img;
using namespace mrpt::math;

/*---------------------------------------------------------------
				Constructor
 ---------------------------------------------------------------*/
CMappedImage::CMappedImage(
	CImage::Ptr img, double x0, double x1, double y0, double y1,
	TInterpolationMethod method)
	: m_img(img),
	  m_x0(x0),
	  m_x1(x1),
	  m_y0(y0),
	  m_y1(y1),
	  m_pixel_size(0),
	  m_method(method)
{
	m_img->grayscale();
	if (m_img->isColor())
	{
		auto* new_img = new CImage();
		m_img->grayscale(*new_img);
		m_img = CImage::Ptr(new_img);
	}
	changeCoordinates(x0, x1, y0, y1);
}

/*---------------------------------------------------------------
				changeCoordinates
 ---------------------------------------------------------------*/
void CMappedImage::changeCoordinates(double x0, double x1, double y0, double y1)
{
	MRPT_START
	ASSERT_(x0 != x1);
	ASSERT_(y0 != y1);

	m_x0 = x0;
	m_x1 = x1;
	m_y0 = y0;
	m_y1 = y1;

	if (y1 < 0 || x1 < 0)
	{
		m_x1 = m_img->getWidth() - 1;
		m_y1 = m_img->getHeight() - 1;
	}

	ASSERT_(m_img->getWidth() > 0 && m_img->getHeight());

	m_pixel_size = (m_x1 - m_x0) / m_img->getWidth();

	MRPT_END
}

/*---------------------------------------------------------------
				getPixel
 ---------------------------------------------------------------*/
double CMappedImage::getPixel(double x, double y) const
{
	// Image size:
	const size_t W = m_img->getWidth();
	const size_t H = m_img->getHeight();

	// the sub-pixel pixel coordinates:
	const double px = (x - m_x0) / m_pixel_size;
	const double py = (y - m_y0) / m_pixel_size;

	if (px < 0 || py < 0 || px > W || py > H) { return 0; }	 // Out of image

	switch (m_method)
	{
		case IMG_INTERP_NN:
		{
			// The closest pixel:
			const unsigned int px0 = mrpt::round(px);
			const unsigned int py0 = mrpt::round(py);
			return static_cast<double>((*m_img).at<uint8_t>(px0, py0));
		}
		break;
		case IMG_INTERP_LINEAR:
		{
			// See: http://en.wikipedia.org/wiki/Bilinear_interpolation

			// The four pixels around:
			const int px0 = (int)floor(px);
			const int px1 = (int)ceil(px);
			const int py0 = (int)floor(py);
			const int py1 = (int)ceil(py);

			const auto P11 =
				static_cast<double>((*m_img).at<uint8_t>(px0, py0));
			const auto P12 =
				static_cast<double>((*m_img).at<uint8_t>(px0, py1));
			const auto P21 =
				static_cast<double>((*m_img).at<uint8_t>(px1, py0));
			const auto P22 =
				static_cast<double>((*m_img).at<uint8_t>(px1, py1));

			const double R1 = P11 * (px1 - px) /* /(px1-px0)*/ +
				P21 * (px - px0) /* /(px1-px0) */;
			const double R2 = P12 * (px1 - px) /* /(px1-px0)*/ +
				P22 * (px - px0) /* /(px1-px0) */;

			return R1 * (py1 - py) + R2 * (py - py0);
		}
		break;

		case IMG_INTERP_CUBIC:
		case IMG_INTERP_AREA:
		default:
			THROW_EXCEPTION(
				"The selected interpolation method is not supported in this "
				"method.");
	};
}
