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

#include "base-precomp.h"  // Precompiled headers

#include <mrpt/utils/CMappedImage.h>
#include <mrpt/utils/round.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::math;


/*---------------------------------------------------------------
                Constructor
 ---------------------------------------------------------------*/
CMappedImage::CMappedImage( CImagePtr 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())
	{
		CImage *new_img = new CImage();
		m_img->grayscale(*new_img);
		m_img = CImagePtr( 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 int px0 = mrpt::utils::round(px);
			const int py0 = mrpt::utils::round(py);
			return static_cast<double>(*m_img->get_unsafe(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 double P11 = static_cast<double>(*m_img->get_unsafe(px0, py0));
			const double P12 = static_cast<double>(*m_img->get_unsafe(px0, py1));
			const double P21 = static_cast<double>(*m_img->get_unsafe(px1, py0));
			const double P22 = static_cast<double>(*m_img->get_unsafe(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:
		{
			THROW_EXCEPTION("TO DO!");
		}
		break;

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

}

