/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, 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;);
}
