/* +------------------------------------------------------------------------+
   |                     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/math/CMatrixDynamic.h>
#include <mrpt/opengl/CSetOfTriangles.h>
#include <mrpt/opengl/opengl_api.h>
#include <mrpt/serialization/CArchive.h>

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

IMPLEMENTS_SERIALIZABLE(CSetOfTriangles, CRenderizableShaderTriangles, mrpt::opengl)

void CSetOfTriangles::onUpdateBuffers_Triangles()
{
  // Nothing else to do, data is directly kept up-to-date in base class
  // buffers.
}

void CSetOfTriangles::clearTriangles()
{
  std::unique_lock<std::shared_mutex> trisWriteLock(
      CRenderizableShaderTriangles::m_trianglesMtx.data);
  auto& tris = CRenderizableShaderTriangles::m_triangles;
  tris.clear();
  polygonsUpToDate = false;
  CRenderizable::notifyChange();
}

size_t CSetOfTriangles::getTrianglesCount() const
{
  std::shared_lock<std::shared_mutex> trisReadLock(
      CRenderizableShaderTriangles::m_trianglesMtx.data);
  return shaderTrianglesBuffer().size();
}

uint8_t CSetOfTriangles::serializeGetVersion() const { return 0; }
void CSetOfTriangles::serializeTo(mrpt::serialization::CArchive& out) const
{
  writeToStreamRender(out);

  std::shared_lock<std::shared_mutex> trisReadLock(
      CRenderizableShaderTriangles::m_trianglesMtx.data);

  auto n = (uint32_t)shaderTrianglesBuffer().size();
  out << n;
  for (size_t i = 0; i < n; i++) shaderTrianglesBuffer()[i].writeTo(out);
}
void CSetOfTriangles::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  std::unique_lock<std::shared_mutex> trisLck(CRenderizableShaderTriangles::m_trianglesMtx.data);
  auto& tris = CRenderizableShaderTriangles::m_triangles;

  switch (version)
  {
    case 0:
    {
      readFromStreamRender(in);
      uint32_t n;
      in >> n;
      tris.assign(n, TTriangle());
      for (size_t i = 0; i < n; i++) tris[i].readFrom(in);
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
  polygonsUpToDate = false;
  CRenderizable::notifyChange();
}

bool CSetOfTriangles::traceRay(const mrpt::poses::CPose3D& o, double& dist) const
{
  if (!polygonsUpToDate) updatePolygons();
  return mrpt::math::traceRay(m_polygons, (o - getCPose()).asTPose(), dist);
}
CRenderizable& CSetOfTriangles::setColor_u8(const mrpt::img::TColor& c)
{
  CRenderizable::notifyChange();
  setColor_u8(c);
  std::unique_lock<std::shared_mutex> trisLck(CRenderizableShaderTriangles::m_trianglesMtx.data);
  auto& tris = CRenderizableShaderTriangles::m_triangles;

  for (auto& t : tris) t.setColor(c);
  return *this;
}

CRenderizable& CSetOfTriangles::setColorA_u8(const uint8_t a)
{
  CRenderizable::notifyChange();
  setColorA_u8(a);
  std::unique_lock<std::shared_mutex> trisLck(CRenderizableShaderTriangles::m_trianglesMtx.data);
  auto& tris = CRenderizableShaderTriangles::m_triangles;

  for (auto& t : tris) t.setColor(getColor_u8());
  return *this;
}

void CSetOfTriangles::getPolygons(std::vector<mrpt::math::TPolygon3D>& polys) const
{
  if (!polygonsUpToDate) updatePolygons();
  size_t N = m_polygons.size();
  for (size_t i = 0; i < N; i++) polys[i] = m_polygons[i].poly;
}

void CSetOfTriangles::updatePolygons() const
{
  std::unique_lock<std::shared_mutex> trisLck(CRenderizableShaderTriangles::m_trianglesMtx.data);
  auto& tris = CRenderizableShaderTriangles::m_triangles;

  TPolygon3D tmp(3);
  size_t N = tris.size();
  m_polygons.resize(N);
  for (size_t i = 0; i < N; i++)
    for (size_t j = 0; j < 3; j++)
    {
      const TTriangle& t = tris[i];
      tmp[j].x = t.x(j);
      tmp[j].y = t.y(j);
      tmp[j].z = t.z(j);
      m_polygons[i] = tmp;
    }
  polygonsUpToDate = true;
  CRenderizable::notifyChange();
}

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

void CSetOfTriangles::insertTriangles(const CSetOfTriangles::Ptr& p)
{
  ASSERT_(p);

  std::unique_lock<std::shared_mutex> trisLck(CRenderizableShaderTriangles::m_trianglesMtx.data);

  std::shared_lock<std::shared_mutex> trisOtherLck(p->m_trianglesMtx.data);

  auto& tris = CRenderizableShaderTriangles::m_triangles;
  auto& trisOther = p->shaderTrianglesBuffer();

  reserve(tris.size() + trisOther.size());
  tris.insert(tris.end(), trisOther.begin(), trisOther.end());
  polygonsUpToDate = false;
  CRenderizable::notifyChange();
}
