/* +------------------------------------------------------------------------+
   |                     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 "opengl-precomp.h"  // Precompiled header
//
#include <mrpt/img/color_maps.h>
#include <mrpt/opengl/CMesh3D.h>
#include <mrpt/opengl/opengl_api.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CMesh3D, CRenderizable, mrpt::opengl)

CMesh3D::~CMesh3D() = default;

void CMesh3D::loadMesh(
    unsigned int num_verts,
    unsigned int num_faces,
    int* verts_per_face,
    int* face_verts,
    float* vert_coords)
{
  // Fill number of vertices for each face
  m_is_quad.resize(num_faces);
  for (unsigned int i = 0; i < num_faces; i++)
  {
    if (verts_per_face[i] == 3)
      m_is_quad[i] = false;
    else if (verts_per_face[i] == 4)
      m_is_quad[i] = true;
    else
    {
      THROW_EXCEPTION(
          "Incorrect mesh format. It can only be composed of triangles "
          "and/or quads");
    }
  }

  // Fill the vertices of each face
  m_face_verts.resize(num_faces);
  unsigned int count = 0;
  for (unsigned int f = 0; f < num_faces; f++)
  {
    m_face_verts[f][0] = face_verts[count++];
    m_face_verts[f][1] = face_verts[count++];
    m_face_verts[f][2] = face_verts[count++];
    if (m_is_quad[f])
      m_face_verts[f][3] = face_verts[count++];
    else
      m_face_verts[f][3] = -1;  // Meaning it is a triangle
  }

  // Fill the 3D coordinates of the vertex
  m_vertices.resize(num_verts);
  for (unsigned int i = 0; i < num_verts; i++)
  {
    m_vertices[i][0] = vert_coords[3 * i];
    m_vertices[i][1] = vert_coords[3 * i + 1];
    m_vertices[i][2] = vert_coords[3 * i + 2];
  }

  // Compute the mesh normals (if on)
  if (m_computeNormals)
  {
    m_normals.resize(num_faces);

    for (unsigned int f = 0; f < num_faces; f++)
    {
      const unsigned int v1 = m_face_verts[f][3];
      const unsigned int v2 = m_face_verts[f][2];
      const unsigned int v3 = m_face_verts[f][1];
      const unsigned int v4 = m_face_verts[f][0];

      if (m_is_quad[f])
      {
        const float vec1[3] = {
            m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1],
            m_vertices[v3][2] - m_vertices[v1][2]};
        const float vec2[3] = {
            m_vertices[v4][0] - m_vertices[v2][0], m_vertices[v4][1] - m_vertices[v2][1],
            m_vertices[v4][2] - m_vertices[v2][2]};
        m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
        m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
        m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
      }
      else
      {
        const float vec1[3] = {
            m_vertices[v2][0] - m_vertices[v1][0], m_vertices[v2][1] - m_vertices[v1][1],
            m_vertices[v2][2] - m_vertices[v1][2]};
        const float vec2[3] = {
            m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1],
            m_vertices[v3][2] - m_vertices[v1][2]};
        m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
        m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
        m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
      }
      m_normals[f] = m_normals[f].unitarize();
    }
  }

  CRenderizable::notifyChange();
}

void CMesh3D::loadMesh(
    unsigned int num_verts,
    unsigned int num_faces,
    const mrpt::math::CMatrixDynamic<bool>& is_quad,
    const mrpt::math::CMatrixDynamic<int>& face_verts,
    const mrpt::math::CMatrixDynamic<float>& vert_coords)
{
  // Fill number of vertices for each face
  m_is_quad.resize(num_faces);
  for (unsigned int i = 0; i < num_faces; i++) m_is_quad[i] = is_quad(i, 0);

  // Fill the vertices of each face
  m_face_verts.resize(num_faces);
  for (unsigned int f = 0; f < num_faces; f++)
  {
    m_face_verts[f][0] = face_verts(0, f);
    m_face_verts[f][1] = face_verts(1, f);
    m_face_verts[f][2] = face_verts(2, f);
    if (m_is_quad[f])
      m_face_verts[f][3] = face_verts(3, f);
    else
      m_face_verts[f][3] = -1;  // Meaning it is a triangle
  }

  // Fill the 3D coordinates of the vertex
  m_vertices.resize(num_verts);
  for (unsigned int i = 0; i < num_verts; i++)
  {
    m_vertices[i][0] = vert_coords(0, i);
    m_vertices[i][1] = vert_coords(1, i);
    m_vertices[i][2] = vert_coords(2, i);
  }

  // Compute the mesh normals (if on)
  m_normals.resize(num_faces);
  if (m_computeNormals)
    for (unsigned int f = 0; f < num_faces; f++)
    {
      const unsigned int v1 = m_face_verts[f][0];
      const unsigned int v2 = m_face_verts[f][1];
      const unsigned int v3 = m_face_verts[f][2];
      const unsigned int v4 = m_face_verts[f][3];

      if (m_is_quad[f])
      {
        const float vec1[3] = {
            m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1],
            m_vertices[v3][2] - m_vertices[v1][2]};
        const float vec2[3] = {
            m_vertices[v4][0] - m_vertices[v2][0], m_vertices[v4][1] - m_vertices[v2][1],
            m_vertices[v4][2] - m_vertices[v2][2]};
        m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
        m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
        m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
      }
      else
      {
        const float vec1[3] = {
            m_vertices[v2][0] - m_vertices[v1][0], m_vertices[v2][1] - m_vertices[v1][1],
            m_vertices[v2][2] - m_vertices[v1][2]};
        const float vec2[3] = {
            m_vertices[v3][0] - m_vertices[v1][0], m_vertices[v3][1] - m_vertices[v1][1],
            m_vertices[v3][2] - m_vertices[v1][2]};
        m_normals[f][0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
        m_normals[f][1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
        m_normals[f][2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
      }
    }

  CRenderizable::notifyChange();
}

void CMesh3D::render(const RenderContext& rc) const
{
  switch (rc.shader_id)
  {
    case DefaultShaderID::TRIANGLES_LIGHT:
      if (m_showFaces) CRenderizableShaderTriangles::render(rc);
      break;
    case DefaultShaderID::WIREFRAME:
      if (m_showEdges) CRenderizableShaderWireFrame::render(rc);
      break;
    case DefaultShaderID::POINTS:
      if (m_showVertices) CRenderizableShaderPoints::render(rc);
      break;
  };
}
void CMesh3D::renderUpdateBuffers() const
{
  CRenderizableShaderPoints::renderUpdateBuffers();
  CRenderizableShaderTriangles::renderUpdateBuffers();
  CRenderizableShaderWireFrame::renderUpdateBuffers();
}

void CMesh3D::onUpdateBuffers_Wireframe()
{
  auto& vbd = CRenderizableShaderWireFrame::m_vertex_buffer_data;
  auto& cbd = CRenderizableShaderWireFrame::m_color_buffer_data;
  std::unique_lock<std::shared_mutex> wfWriteLock(
      CRenderizableShaderWireFrame::m_wireframeMtx.data);

  vbd.clear();

  for (size_t f = 0; f < m_face_verts.size(); f++)
  {
    const unsigned char num_vert = 3 + m_is_quad[f];
    for (int i = 0; i < num_vert - 1; i++)
    {
      const unsigned int v_0 = m_face_verts[f][i];
      const unsigned int v_1 = m_face_verts[f][i + 1];

      vbd.emplace_back(m_vertices[v_0]);
      vbd.emplace_back(m_vertices[v_1]);
    }

    // The last vertex of the face needs to be connected to the first as
    // well
    const int v_0 = m_face_verts[f][num_vert - 1];
    const int v_1 = m_face_verts[f][0];

    vbd.emplace_back(m_vertices[v_0]);
    vbd.emplace_back(m_vertices[v_1]);
  }

  cbd.assign(vbd.size(), edge_color.asTColor());
}

void CMesh3D::onUpdateBuffers_Triangles()
{
  std::unique_lock<std::shared_mutex> trisWriteLock(
      CRenderizableShaderTriangles::m_trianglesMtx.data);
  auto& tris = CRenderizableShaderTriangles::m_triangles;

  tris.clear();

  for (size_t f = 0; f < m_is_quad.size(); f++)
  {
    // Assign normals to faces (if on)
    const auto& normal = m_normals[f];

    tris.emplace_back(
        m_vertices[m_face_verts[f][0]], m_vertices[m_face_verts[f][1]],
        m_vertices[m_face_verts[f][2]], normal, normal, normal);
    if (m_is_quad[f])
    {
      tris.emplace_back(
          m_vertices[m_face_verts[f][0]], m_vertices[m_face_verts[f][2]],
          m_vertices[m_face_verts[f][3]], normal, normal, normal);
    }
  }

  for (auto& t : tris) t.setColor(face_color);

  notifyBBoxChange();
}
void CMesh3D::onUpdateBuffers_Points()
{
  auto& vbd = CRenderizableShaderPoints::m_vertex_buffer_data;
  auto& cbd = CRenderizableShaderPoints::m_color_buffer_data;
  std::unique_lock<std::shared_mutex> wfWriteLock(CRenderizableShaderPoints::m_pointsMtx.data);

  vbd = m_vertices;
  cbd.assign(m_vertices.size(), vert_color.asTColor());
}

uint8_t CMesh3D::serializeGetVersion() const { return 1; }
void CMesh3D::serializeTo(mrpt::serialization::CArchive& out) const
{
  writeToStreamRender(out);
  out << m_showEdges << m_showFaces << m_showVertices << m_computeNormals;
  out << m_is_quad << m_vertices << m_normals;
  out.WriteAs<uint32_t>(m_face_verts.size());
  if (!m_face_verts.empty())
    out.WriteBufferFixEndianness<uint32_t>(
        m_face_verts[0].data(), m_face_verts.size() * m_face_verts[0].size());
  CRenderizableShaderTriangles::params_serialize(out);  // v1
}

void CMesh3D::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    case 1:
    {
      readFromStreamRender(in);
      in >> m_showEdges >> m_showFaces >> m_showVertices >> m_computeNormals;
      in >> m_is_quad >> m_vertices >> m_normals;
      const auto N = in.ReadAs<uint32_t>();
      m_face_verts.resize(N);
      if (!m_face_verts.empty())
        in.ReadBufferFixEndianness<uint32_t>(
            m_face_verts[0].data(), m_face_verts.size() * m_face_verts[0].size());

      if (version >= 1) CRenderizableShaderTriangles::params_deserialize(in);
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
  CRenderizable::notifyChange();
}

auto CMesh3D::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf
{
  return trianglesBoundingBox();
}
