/* +------------------------------------------------------------------------+
   |                     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 "opengl-precomp.h"	 // Precompiled header
//
#include <mrpt/opengl/CGeneralizedEllipsoidTemplate.h>
#include <mrpt/opengl/opengl_api.h>

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

template <>
void CGeneralizedEllipsoidTemplate<2>::implUpdate_Wireframe()
{
	const auto& pts = m_render_pts;

	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();

	// Line loop:
	const auto N = pts.size();
	for (size_t i = 0; i < N; i++)
	{
		const auto ip = (i + 1) % N;
		vbd.emplace_back(pts[i][0], pts[i][1], .0f);
		vbd.emplace_back(pts[ip][0], pts[ip][1], .0f);
	}

	// All lines, same color:
	cbd.assign(vbd.size(), getColor_u8());
}

template <>
void CGeneralizedEllipsoidTemplate<3>::implUpdate_Wireframe()
{
	const auto& pts = m_render_pts;

	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();

	const auto slices = m_numSegments, stacks = m_numSegments;

	// Points in the ellipsoid:
	//  * "#slices" slices, with "#stacks" points each, but for the two ends
	//  * 1 point at each end slice
	// #total points = stacks*(slices-2) + 2
	ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
	const size_t idx_1st_slice = 1;

	// 1st slice: triangle fan (if it were solid)
	// ----------------------------
	for (size_t i = 0; i < stacks; i++)
	{
		const auto idx = idx_1st_slice + i;
		vbd.emplace_back(pts[0]);
		vbd.emplace_back(pts[idx]);
	}

	// Middle slices: triangle strip (if it were solid)
	// ----------------------------
	for (size_t s = 0; s < slices - 3; s++)
	{
		size_t idx0 = idx_1st_slice + stacks * s;
		size_t idx1 = idx0 + stacks;

		for (size_t i = 0; i < stacks; i++)
		{
			const size_t ii =
				(i == (stacks - 1) ? 0 : i + 1);  // next i with wrapping

			vbd.emplace_back(pts[idx0 + i]);
			vbd.emplace_back(pts[idx1 + ii]);

			vbd.emplace_back(pts[idx1 + ii]);
			vbd.emplace_back(pts[idx1 + i]);

			vbd.emplace_back(pts[idx1 + i]);
			vbd.emplace_back(pts[idx0 + i]);

			vbd.emplace_back(pts[idx0 + i]);
			vbd.emplace_back(pts[idx0 + ii]);

			vbd.emplace_back(pts[idx0 + ii]);
			vbd.emplace_back(pts[idx1 + ii]);
		}
	}

	// Last slice: triangle fan (if it were solid)
	// ----------------------------
	const size_t idxN = pts.size() - 1;
	const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;

	for (size_t i = 0; i < stacks; i++)
	{
		vbd.emplace_back(pts[idxN]);
		vbd.emplace_back(pts[idx_last_slice + i]);
	}

	// All lines, same color:
	cbd.assign(vbd.size(), getColor_u8());
}

template <>
void CGeneralizedEllipsoidTemplate<2>::implUpdate_Triangles()
{
	using P3f = mrpt::math::TPoint3Df;
	const auto& pts = m_render_pts;

	// Render precomputed points in m_render_pts:
	std::unique_lock<std::shared_mutex> trisWriteLock(
		CRenderizableShaderTriangles::m_trianglesMtx.data);
	auto& tris = CRenderizableShaderTriangles::m_triangles;

	tris.clear();

	const auto N = pts.size();
	for (size_t i = 0; i < N; i++)
	{
		const auto ip = (i + 1) % N;
		tris.emplace_back(
			P3f(0, 0, 0), P3f(pts[i][0], pts[i][1], .0f),
			P3f(pts[ip][0], pts[ip][1], .0f));
	}

	// All faces, all vertices, same color:
	for (auto& t : tris)
		t.setColor(getColor_u8());
}

template <>
void CGeneralizedEllipsoidTemplate<3>::implUpdate_Triangles()
{
	const auto& pts = m_render_pts;

	// Render precomputed points in m_render_pts:
	std::unique_lock<std::shared_mutex> trisWriteLock(
		CRenderizableShaderTriangles::m_trianglesMtx.data);
	auto& tris = CRenderizableShaderTriangles::m_triangles;

	tris.clear();

	const auto slices = m_numSegments, stacks = m_numSegments;

	// Points in the ellipsoid:
	//  * "#slices" slices, with "#stacks" points each, but for the two ends
	//  * 1 point at each end slice
	// #total points = stacks*(slices-2) + 2
	ASSERT_EQUAL_((slices - 2) * stacks + 2, pts.size());
	const size_t idx_1st_slice = 1;

	// 1st slice: triangle fan (if it were solid)
	// ----------------------------
	for (size_t i = 0; i < stacks; i++)
	{
		const auto idx = idx_1st_slice + i;
		const auto idxp = idx_1st_slice + ((i + 1) % stacks);

		tris.emplace_back(
			// Points
			pts[0], pts[idxp], pts[idx],
			// Normals:
			pts[0], pts[idxp], pts[idx]);
	}

	// Middle slices: triangle strip (if it were solid)
	// ----------------------------
	for (size_t s = 0; s < slices - 3; s++)
	{
		size_t idx0 = idx_1st_slice + stacks * s;
		size_t idx1 = idx0 + stacks;

		for (size_t i = 0; i < stacks; i++)
		{
			const size_t ii =
				(i == (stacks - 1) ? 0 : i + 1);  // next i with wrapping

			tris.emplace_back(
				// Points
				pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i],
				// Normals:
				pts[idx0 + i], pts[idx0 + ii], pts[idx1 + i]);
			tris.emplace_back(
				// Points
				pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii],
				// Normals:
				pts[idx1 + ii], pts[idx1 + i], pts[idx0 + ii]);
		}
	}

	// Last slice: triangle fan (if it were solid)
	// ----------------------------
	const size_t idxN = pts.size() - 1;
	const size_t idx_last_slice = idx_1st_slice + (slices - 3) * stacks;

	for (size_t i = 0; i < stacks; i++)
	{
		const auto idx = idx_last_slice + i;
		const auto idxp = idx_last_slice + ((i + 1) % stacks);

		tris.emplace_back(
			// Points
			pts[idx], pts[idxp], pts[idxN],
			// Normals
			pts[idx], pts[idxp], pts[idxN]);
	}

	// All faces, all vertices, same color:
	for (auto& t : tris)
		t.setColor(getColor_u8());
}

template <>
void CGeneralizedEllipsoidTemplate<2>::generatePoints(
	const CGeneralizedEllipsoidTemplate<2>::cov_matrix_t& U,
	std::vector<CGeneralizedEllipsoidTemplate<2>::array_parameter_t>& pts) const
{
	pts.clear();
	pts.reserve(m_numSegments);
	const double Aa = 2 * M_PI / m_numSegments;
	for (double ang = 0; ang < 2 * M_PI; ang += Aa)
	{
		const double ccos = cos(ang);
		const double ssin = sin(ang);

		pts.resize(pts.size() + 1);

		auto& pt = pts.back();

		pt[0] = d2f(m_mean[0] + ccos * U(0, 0) + ssin * U(0, 1));
		pt[1] = d2f(m_mean[1] + ccos * U(1, 0) + ssin * U(1, 1));
	}
}

inline void aux_add3DpointWithEigenVectors(
	const double x, const double y, const double z,
	std::vector<mrpt::math::CMatrixFixed<float, 3, 1>>& pts,
	const mrpt::math::CMatrixFixed<double, 3, 3>& M,
	const mrpt::math::CMatrixFixed<double, 3, 1>& mean)
{
	pts.resize(pts.size() + 1);
	mrpt::math::CMatrixFixed<float, 3, 1>& pt = pts.back();
	pt[0] = d2f(mean[0] + x * M(0, 0) + y * M(0, 1) + z * M(0, 2));
	pt[1] = d2f(mean[1] + x * M(1, 0) + y * M(1, 1) + z * M(1, 2));
	pt[2] = d2f(mean[2] + x * M(2, 0) + y * M(2, 1) + z * M(2, 2));
}

template <>
void CGeneralizedEllipsoidTemplate<3>::generatePoints(
	const CGeneralizedEllipsoidTemplate<3>::cov_matrix_t& U,
	std::vector<CGeneralizedEllipsoidTemplate<3>::array_parameter_t>& pts) const
{
	MRPT_START
	const auto slices = m_numSegments, stacks = m_numSegments;
	ASSERT_GE_(slices, 3);
	ASSERT_GE_(stacks, 3);
	// sin/cos cache --------
	// Slices: [0,pi]
	std::vector<double> slice_cos(slices), slice_sin(slices);
	for (uint32_t i = 0; i < slices; i++)
	{
		double angle = M_PI * i / double(slices - 1);
		slice_sin[i] = sin(angle);
		slice_cos[i] = cos(angle);
	}
	// Stacks: [0,2*pi]
	std::vector<double> stack_sin(stacks), stack_cos(stacks);
	for (uint32_t i = 0; i < stacks; i++)
	{
		double angle = 2 * M_PI * i / double(stacks);
		stack_sin[i] = sin(angle);
		stack_cos[i] = cos(angle);
	}

	// Points in the ellipsoid:
	//  * "#slices" slices, with "#stacks" points each, but for the two ends
	//  * 1 point at each end slice
	// #total points = stacks*(slices-2) + 2
	pts.clear();
	pts.reserve((slices - 2) * stacks + 2);

	for (uint32_t i = 0; i < slices; i++)
	{
		if (i == 0) aux_add3DpointWithEigenVectors(1, 0, 0, pts, U, m_mean);
		else if (i == (slices - 1))
			aux_add3DpointWithEigenVectors(-1, 0, 0, pts, U, m_mean);
		else
		{
			const double x = slice_cos[i];
			const double R = slice_sin[i];

			for (uint32_t j = 0; j < stacks; j++)
			{
				const double y = R * stack_cos[j];
				const double z = R * stack_sin[j];
				aux_add3DpointWithEigenVectors(x, y, z, pts, U, m_mean);
			}
		}
	}

	MRPT_END
}
