/* +------------------------------------------------------------------------+
   |                     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/CVectorDynamic.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

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

IMPLEMENTS_SERIALIZABLE(CSetOfLines, CRenderizableShaderWireFrame, mrpt::opengl)

/** Constructor */
CSetOfLines::CSetOfLines()
{
  // Override default pointsize=1 in points shader:
  CRenderizableShaderPoints::m_pointSize = 0;
}

/** Constructor with a initial set of lines. */
CSetOfLines::CSetOfLines(const std::vector<TSegment3D>& sgms, bool antiAliasing) : m_Segments(sgms)
{
  // Override default pointsize=1 in points shader:
  CRenderizableShaderPoints::m_pointSize = 0;

  m_lineWidth = 1;
  m_antiAliasing = antiAliasing;
}

void CSetOfLines::render(const RenderContext& rc) const
{
  switch (rc.shader_id)
  {
    case DefaultShaderID::POINTS:
      CRenderizableShaderPoints::render(rc);
      break;
    case DefaultShaderID::WIREFRAME:
      CRenderizableShaderWireFrame::render(rc);
      break;
  };
}
void CSetOfLines::renderUpdateBuffers() const
{
  CRenderizableShaderPoints::renderUpdateBuffers();
  CRenderizableShaderWireFrame::renderUpdateBuffers();
}

/*---------------------------------------------------------------
              setLineByIndex
  ---------------------------------------------------------------*/
void CSetOfLines::setLineByIndex(size_t index, const mrpt::math::TSegment3D& segm)
{
  MRPT_START
  if (index >= m_Segments.size()) THROW_EXCEPTION("Index out of bounds");
  CRenderizable::notifyChange();
  m_Segments[index] = segm;
  MRPT_END
}

float CSetOfLines::getVerticesPointSize() const { return m_pointSize; }
void CSetOfLines::setVerticesPointSize(const float size_points)
{
  m_pointSize = size_points;
  CRenderizable::notifyChange();
}

void CSetOfLines::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();
  vbd.reserve(m_Segments.size() * 2);

  for (const auto& segment : m_Segments)
  {
    vbd.emplace_back(segment.point1);
    vbd.emplace_back(segment.point2);
  }

  // The same color to all vertices:
  cbd.assign(vbd.size(), getColor_u8());
}

void CSetOfLines::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.clear();
  cbd.clear();

  // Only draw points if they are enabled:
  if (m_pointSize <= .0f) return;

  vbd.reserve(m_Segments.size() * 2);

  for (const auto& segment : m_Segments)
  {
    vbd.emplace_back(segment.point1);
    vbd.emplace_back(segment.point2);
  }

  // The same color to all vertices:
  cbd.assign(vbd.size(), getColor_u8());
}

uint8_t CSetOfLines::serializeGetVersion() const { return 4; }
void CSetOfLines::serializeTo(mrpt::serialization::CArchive& out) const
{
  writeToStreamRender(out);
  out << m_Segments << m_lineWidth;
  out << m_antiAliasing;                             // Added in v3
  CRenderizableShaderPoints::params_serialize(out);  // v4
}

void CSetOfLines::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    case 1:
    {
      readFromStreamRender(in);
      CVectorFloat x0, y0, z0, x1, y1, z1;
      in >> x0 >> y0 >> z0 >> x1 >> y1 >> z1;
      if (version >= 1)
        in >> m_lineWidth;
      else
        m_lineWidth = 1;
      size_t N = x0.size();
      m_Segments.resize(N);
      for (size_t i = 0; i < N; i++)
      {
        m_Segments[i][0][0] = x0[i];
        m_Segments[i][0][1] = y0[i];
        m_Segments[i][0][2] = z0[i];
        m_Segments[i][1][0] = x1[i];
        m_Segments[i][1][1] = y1[i];
        m_Segments[i][1][2] = z1[i];
      }
    }
    break;
    case 2:
    case 3:
    case 4:
    {
      readFromStreamRender(in);
      in >> m_Segments;
      in >> m_lineWidth;
      if (version >= 3)
        in >> m_antiAliasing;
      else
        m_antiAliasing = true;
      if (version >= 4)
        CRenderizableShaderPoints::params_deserialize(in);
      else
        m_pointSize = .0f;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
  CRenderizable::notifyChange();
}

auto CSetOfLines::internalBoundingBoxLocal() const -> mrpt::math::TBoundingBoxf
{
  auto bb = mrpt::math::TBoundingBoxf::PlusMinusInfinity();

  for (const auto& s : m_Segments)
  {
    for (size_t p = 0; p < 2; p++)
    {
      const TPoint3D& pt = s[p];
      bb.updateWithPoint(pt);
    }
  }

  return bb;
}

void CSetOfLines::getLineByIndex(
    size_t index, double& x0, double& y0, double& z0, double& x1, double& y1, double& z1) const
{
  ASSERT_(index < m_Segments.size());
  const mrpt::math::TPoint3D& p0 = m_Segments[index].point1;
  const mrpt::math::TPoint3D& p1 = m_Segments[index].point2;
  x0 = p0.x;
  y0 = p0.y;
  z0 = p0.z;
  x1 = p1.x;
  y1 = p1.y;
  z1 = p1.z;
}
