/* +------------------------------------------------------------------------+
   |                     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 "img-precomp.h"  // Precompiled headers
//
#include <mrpt/core/reverse_bytes.h>
#include <mrpt/core/round.h>
#include <mrpt/img/CCanvas.h>
#include <mrpt/img/CImage.h>
#include <mrpt/io/zip.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <Eigen/Dense>
#include <cstring>	// memcpy
#include <map>

// Include the MRPT bitmap fonts:
#include "mrpt_font_10x20.h"
#include "mrpt_font_5x7.h"
#include "mrpt_font_6x13.h"
#include "mrpt_font_6x13B.h"
#include "mrpt_font_6x13O.h"
#include "mrpt_font_9x15.h"
#include "mrpt_font_9x15B.h"

// Japanese fonts?
#if MRPT_HAS_ASIAN_FONTS
#include "mrpt_font_18x18ja.h"
#endif

// Each font has a block a data with this header (It's actually zip-compressed
// since mrpt >0.6.5)
//	const uint32_t mrpt_font_9x15B [] = {
//	9,15, /* width, height */
//	0x0000,0x00FF, /* UNICODE characters range: */

using namespace mrpt;
using namespace mrpt::img;
using namespace std;

struct FontData
{
	std::vector<uint8_t> data;
	bool prepared_to_big_endian = false;
};

// Each vector is the target place where to uncompress each font.
map<string, FontData> list_registered_fonts;
bool list_fonts_init = false;

void init_fonts_list()
{
	if (!list_fonts_init)
	{
		list_registered_fonts.clear();

// This was used only once
#if 0
#define SAVE_COMPRESSED(ARR)                                                   \
	{                                                                          \
		list_registered_fonts[#ARR].resize(sizeof(mrpt_font_##ARR));           \
		memcpy(                                                                \
			&list_registered_fonts[#ARR][0], mrpt_font_##ARR,                  \
			sizeof(mrpt_font_##ARR));                                          \
		cout << #ARR << " -> " << sizeof(mrpt_font_##ARR) << endl;             \
		CFileGZOutputStream f(                                                 \
			string("mrpt_font_") + string(#ARR) + string(".gz"));              \
		f.WriteBuffer(mrpt_font_##ARR, sizeof(mrpt_font_##ARR));               \
		/*mrpt::compress::zip::compress( list_registered_fonts[#ARR], f ); */  \
	}

    	SAVE_COMPRESSED(5x7)
//    	SAVE_COMPRESSED(6x13)
//    	SAVE_COMPRESSED(6x13B)
//    	SAVE_COMPRESSED(6x13O)
//    	SAVE_COMPRESSED(9x15)
//    	SAVE_COMPRESSED(9x15B)
//    	SAVE_COMPRESSED(10x20)

#if MRPT_HAS_ASIAN_FONTS
//    	SAVE_COMPRESSED(18x18ja)
#endif

#endif

#if 1  // Normal operation: Load fonts and uncompress them:

#define LOAD_FONT(FONTNAME)                                                    \
	{                                                                          \
		std::vector<uint8_t> tmpBuf(sizeof(mrpt_font_gz_##FONTNAME));          \
		memcpy(                                                                \
			&tmpBuf[0], mrpt_font_gz_##FONTNAME,                               \
			sizeof(mrpt_font_gz_##FONTNAME));                                  \
		mrpt::io::zip::decompress_gz_data_block(                               \
			tmpBuf, list_registered_fonts[#FONTNAME].data);                    \
	}

		LOAD_FONT(5x7)
		LOAD_FONT(6x13)
		LOAD_FONT(6x13B)
		LOAD_FONT(6x13O)
		LOAD_FONT(9x15)
		LOAD_FONT(9x15B)
		LOAD_FONT(10x20)
#if MRPT_HAS_ASIAN_FONTS
		LOAD_FONT(18x18ja)
#endif

#endif

		list_fonts_init = true;
	}
}

/*---------------------------------------------------------------
						line
---------------------------------------------------------------*/
void CCanvas::line(
	int x0, int y0, int x1, int y1, const mrpt::img::TColor color,
	[[maybe_unused]] unsigned int width, [[maybe_unused]] TPenStyle penStyle)
{
	float x, y;

	auto Ax = (float)(x1 - x0);
	auto Ay = (float)(y1 - y0);

	// In this cases, there is nothing to do!
	if (Ax == 0 && Ay == 0) return;
	if (x0 < 0 && x1 < 0) return;
	if (y0 < 0 && y1 < 0) return;
	if (x0 >= (int)getWidth() && x1 >= (int)getWidth()) return;
	if (y0 >= (int)getHeight() && y1 >= (int)getHeight()) return;

	float dist = sqrt(square(Ax) + square(Ay));
	int i, N = (int)ceil(dist);

	// The N steps to perform next:
	Ax /= N;
	Ay /= N;
	x = (float)x0;
	y = (float)y0;

	for (i = 0; i < N; i++)
	{
		x += Ax;
		y += Ay;
		setPixel((int)x, (int)y, color);
	}  // end for i
}

/*---------------------------------------------------------------
						rectangle
---------------------------------------------------------------*/
void CCanvas::rectangle(
	int x0, int y0, int x1, int y1, const mrpt::img::TColor color,
	unsigned int width)
{
	int w_min = (int)-ceil(((float)width) / 2);
	int w_max = (int)floor(((float)width) / 2);
	// Draw "width" rectangles one into another:
	for (int w = w_min; w <= w_max; w++)
	{
		line(x0 - w, y0 - w, x1 + w, y0 - w, color);
		line(x1 + w, y0 - w, x1 + w, y1 + w, color);
		line(x1 + w, y1 + w, x0 - w, y1 + w, color);
		line(x0 - w, y1 + w, x0 - w, y0 - w, color);
	}  // end for "w"
}

/*****************************************************AJOGD***************************************************/
/*---------------------------------------------------------------
						triangle
---------------------------------------------------------------*/
void CCanvas::triangle(
	int x0, int y0, int size, const mrpt::img::TColor color, bool inferior,
	unsigned int width)
{
	int ts = round(0.866 * size);
	int tc = round(0.5 * size);
	if (inferior)
	{
		line(x0, y0 + size, x0 + ts, y0 - tc, color, width);
		line(x0, y0 + size, x0 - ts, y0 - tc, color, width);
		line(x0 + ts, y0 - tc, x0 - ts, y0 - tc, color, width);
	}
	else
	{
		line(x0, y0 - size, x0 + ts, y0 + tc, color, width);
		line(x0, y0 - size, x0 - ts, y0 + tc, color, width);
		line(x0 + ts, y0 + tc, x0 - ts, y0 + tc, color, width);
	}
}
/************************************************************************************************************/

/*---------------------------------------------------------------
						filledRectangle
---------------------------------------------------------------*/
void CCanvas::filledRectangle(
	int x0, int y0, int x1, int y1, const mrpt::img::TColor color)
{
	int x_min = max(x0, 0);
	int x_max = min(x1, (int)getWidth() - 1);
	int y_min = max(y0, 0);
	int y_max = min(y1, (int)getHeight() - 1);

	for (int y = y_min; y <= y_max; y++)
		for (int x = x_min; x <= x_max; x++)
			setPixel(x, y, color);
}

/*---------------------------------------------------------------
					selectTextFont
---------------------------------------------------------------*/
void CCanvas::selectTextFont(const std::string& fontName)
{
	init_fonts_list();

	// Assure list name is in the list:
	auto it = list_registered_fonts.find(fontName);
	if (it == list_registered_fonts.end())
	{
		// Error
		cerr << "[CCanvas::selectTextFont] Warning: Unknown font: " << fontName
			 << endl;
		return;
	}
	else
	{
		FontData& fd = it->second;
		m_selectedFontBitmaps = reinterpret_cast<const uint32_t*>(&fd.data[0]);
		m_selectedFont = fontName;

#if MRPT_IS_BIG_ENDIAN
		// Fix endianness of char tables:
		if (!fd.prepared_to_big_endian)
		{
			fd.prepared_to_big_endian = true;  // Only do once
			uint32_t* ptr = reinterpret_cast<uint32_t*>(&fd.data[0]);
			for (size_t i = 0; i < fd.data.size() / sizeof(uint32_t); i++)
				mrpt::reverseBytesInPlace(ptr[i]);
		}
#endif
	}
}

/*---------------------------------------------------------------
						drawImage
---------------------------------------------------------------*/
void CCanvas::drawImage(int x, int y, const mrpt::img::CImage& img)
{
	MRPT_START
	ASSERT_(img.getPixelDepth() == mrpt::img::PixelDepth::D8U);

	int img_lx = img.getWidth();
	int img_ly = img.getHeight();

	if (img.isColor())
	{
		for (int xx = 0; xx < img_lx; xx++)
			for (int yy = 0; yy < img_ly; yy++)
			{
				auto ptr = img(xx, yy);
				const int p = ptr[0] | (ptr[1] << 8) | (ptr[2] << 16);
				setPixel(x + xx, y + yy, p);
			}
	}
	else
	{
		unsigned char c;
		int col;
		for (int xx = 0; xx < img_lx; xx++)
			for (int yy = 0; yy < img_ly; yy++)
			{
				c = *((unsigned char*)img(xx, yy));
				col = c | (c << 8) | (c << 16);
				setPixel(x + xx, y + yy, col);
			}
	}

	MRPT_END
}

/*---------------------------------------------------------------
						drawImage
---------------------------------------------------------------*/
void CCanvas::drawImage(
	[[maybe_unused]] int x, [[maybe_unused]] int y,
	[[maybe_unused]] const mrpt::img::CImage& img,
	[[maybe_unused]] float rotation, [[maybe_unused]] float scale)
{
	MRPT_START

	THROW_EXCEPTION("Not implemented yet!! Try yourself! ;-)");

	MRPT_END
}

void CCanvas::drawMark(
	int x0, int y0, const mrpt::img::TColor color, char type, int size,
	unsigned int width)
{
	switch (type)
	{
		case '+':
			line(x0 - size, y0, x0 + size, y0, color, width);
			line(x0, y0 - size, x0, y0 + size, color, width);
			break;
		case 's':
			line(x0 - size, y0 - size, x0 + size, y0 - size, color, width);
			line(x0 + size, y0 - size, x0 + size, y0 + size, color, width);
			line(x0 - size, y0 + size, x0 + size, y0 + size, color, width);
			line(x0 - size, y0 - size, x0 - size, y0 + size, color, width);
			break;
		case 'x':
			line(x0 - size, y0 - size, x0 + size, y0 + size, color, width);
			line(x0 + size, y0 - size, x0 - size, y0 + size, color, width);
			break;
		case ':':
			line(x0 - size, y0, x0 - 2, y0, color, width);
			line(x0 + 2, y0, x0 + size, y0, color, width);
			line(x0, y0 - size, x0, y0 - 2, color, width);
			line(x0, y0 + 2, x0, y0 + size, color, width);
			break;
		default: THROW_EXCEPTION("Unexpected 'type' of cross to be drawn");
	}
}

/*---------------------------------------------------------------
						drawCircle
---------------------------------------------------------------*/
void CCanvas::drawCircle(
	int x, int y, int radius, const mrpt::img::TColor& color,
	unsigned int width)
{
	if (radius < 0) radius = -radius;

	int nSegments;

	if (radius == 0) { nSegments = 2; }
	else
	{
		nSegments = int(M_2PI * radius);
	}

	int x1 = 0, y1 = 0, x2 = 0, y2 = 0;
	double ang, Aa = M_2PI / (nSegments - 1);
	int i;

	for (i = 0, ang = 0; i < nSegments; i++, ang += Aa)
	{
		x2 = round(x + radius * cos(ang));
		y2 = round(y + radius * sin(ang));

		if (i > 0) line(x1, y1, x2, y2, color, width);

		x1 = x2;
		y1 = y2;
	}  // end for points on ellipse
}

/*---------------------------------------------------------------
						textOut
---------------------------------------------------------------*/
void CCanvas::textOut(
	int x0, int y0, const std::string& str, const mrpt::img::TColor color)
{
	MRPT_START

	if (!m_selectedFontBitmaps)	 // First call: load fonts
		this->selectTextFont("9x15");

	// Am I an image?
	bool y_axis_reversed = false;
	auto* im_image = dynamic_cast<CImage*>(this);
	if (im_image) y_axis_reversed = !im_image->isOriginTopLeft();

	// Decode UNICODE string:
	std::vector<uint16_t> uniStr;
	mrpt::system::decodeUTF8(str, uniStr);

	int px = x0;
	int py = y0;

	// Char size:
	int char_w = m_selectedFontBitmaps[0];
	int char_h = m_selectedFontBitmaps[1];

	for (unsigned short unichar : uniStr)
	{
		// look for the character in the table:
		const uint32_t* table_ptr = m_selectedFontBitmaps + 2;
		uint32_t charset_ini = table_ptr[0];
		uint32_t charset_end = table_ptr[1];

		while (charset_end)
		{
			// Is in this range?
			if (unichar <= charset_end && unichar >= charset_ini)
			{
				// Draw this character:
				int pyy = y_axis_reversed ? (py + char_h - 1) : py;

				const uint32_t* char_bitmap =
					table_ptr + 2 + char_h * (unichar - charset_ini);

				for (int y = 0; y < char_h;
					 y++, pyy += y_axis_reversed ? -1 : 1)
				{
					// Use memcpy() here since directly dereferencing is an
					// invalid operation in architectures (S390X) where
					// unaligned accesses are forbiden:
					uint32_t row;
					memcpy(&row, char_bitmap, sizeof(row));
					char_bitmap++;
					for (int x = 0, pxx = px; x < char_w; x++, pxx++)
						if (!!(row & (1 << x))) setPixel(pxx, pyy, color);
				}

				// Advance the raster cursor:
				px += char_w;

				// Next char!
				break;
			}
			else
			{
				// No: Move to the next block and keep searching:
				uint32_t n_chars = charset_end - charset_ini + 1;
				table_ptr += 2 /* Header */ + n_chars * char_h;

				// get new block header:
				charset_ini = table_ptr[0];
				charset_end = table_ptr[1];
			}
		}
		// Char not in the font!
	}

	MRPT_END
}

void CCanvas::ellipseGaussian(
	const mrpt::math::CMatrixFixed<double, 2, 2>& cov2D, const double mean_x,
	const double mean_y, double confIntervalStds,
	const mrpt::img::TColor& color, unsigned int width, int nEllipsePoints)
{
	MRPT_START
	int x1 = 0, y1 = 0, x2 = 0, y2 = 0;
	double ang;
	mrpt::math::CMatrixFixed<double, 2, 2> eigVec, eigVals;
	std::vector<double> eVals;
	int i;

	// Compute the eigen-vectors & values:
	cov2D.eig(eigVec, eVals);
	eigVals.setDiagonal(eVals);

	eigVals.asEigen() = eigVals.array().sqrt().matrix();

	mrpt::math::CMatrixFixed<double, 2, 2> M;
	M.asEigen() = eigVals.asEigen() * eigVec.transpose();

	// Compute the points of the 2D ellipse:
	for (i = 0, ang = 0; i < nEllipsePoints;
		 i++, ang += (M_2PI / (nEllipsePoints - 1)))
	{
		double ccos = cos(ang);
		double ssin = sin(ang);

		x2 = round(
			mean_x + confIntervalStds * (ccos * M(0, 0) + ssin * M(1, 0)));
		y2 = round(
			mean_y + confIntervalStds * (ccos * M(0, 1) + ssin * M(1, 1)));

		if (i > 0) line(x1, y1, x2, y2, color, width);

		x1 = x2;
		y1 = y2;
	}  // end for points on ellipse

	MRPT_END_WITH_CLEAN_UP(std::cout << "Covariance matrix leading to error is:"
									 << std::endl
									 << cov2D << std::endl;);
}
