/* +------------------------------------------------------------------------+
   |                     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 "nav-precomp.h"  // Precomp header
//
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt::nav;

CPTG_RobotShape_Polygonal::CPTG_RobotShape_Polygonal() : m_robotShape() {}
CPTG_RobotShape_Polygonal::~CPTG_RobotShape_Polygonal() = default;
void CPTG_RobotShape_Polygonal::setRobotShape(
	const mrpt::math::CPolygon& robotShape)
{
	ASSERT_GE_(robotShape.size(), 3u);
	m_robotShape = robotShape;

	m_robotMaxRadius = .0;	// Default minimum
	for (const auto& v : m_robotShape)
		mrpt::keep_max(m_robotMaxRadius, v.norm());

	internal_processNewRobotShape();
}

void CPTG_RobotShape_Polygonal::loadDefaultParams()
{
	m_robotShape.clear();
	m_robotShape.AddVertex(-0.15, 0.15);
	m_robotShape.AddVertex(0.2, 0.1);
	m_robotShape.AddVertex(0.2, -0.1);
	m_robotShape.AddVertex(-0.15, -0.15);
}

void CPTG_RobotShape_Polygonal::loadShapeFromConfigFile(
	const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
{
	bool any_pt = false;
	const double BADNUM = std::numeric_limits<double>::max();

	for (unsigned int nPt = 0;; ++nPt)
	{
		const std::string sPtx = mrpt::format("shape_x%u", nPt);
		const std::string sPty = mrpt::format("shape_y%u", nPt);

		const double ptx = cfg.read_double(sSection, sPtx, BADNUM, false);
		const double pty = cfg.read_double(sSection, sPty, BADNUM, false);
		if (ptx == BADNUM && pty == BADNUM) break;
		ASSERTMSG_(
			(ptx != BADNUM && pty != BADNUM),
			"Error: mismatch between number of pts in {x,y} defining robot "
			"shape");

		if (!any_pt)
		{
			m_robotShape.clear();
			any_pt = true;
		}

		m_robotShape.AddVertex(ptx, pty);
	}

	if (any_pt) internal_processNewRobotShape();
}

void CPTG_RobotShape_Polygonal::saveToConfigFile(
	mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
{
	const int WN = 25, WV = 30;

	for (unsigned int i = 0; i < m_robotShape.size(); i++)
	{
		const std::string sPtx = mrpt::format("shape_x%u", i);
		const std::string sPty = mrpt::format("shape_y%u", i);

		cfg.write(
			sSection, sPtx, m_robotShape[i].x, WN, WV,
			"Robot polygonal shape, `x` [m].");
		cfg.write(
			sSection, sPty, m_robotShape[i].y, WN, WV,
			"Robot polygonal shape, `y` [m].");
	}
}

void CPTG_RobotShape_Polygonal::static_add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin,
	const mrpt::math::CPolygon& robotShape)
{
	const int N = robotShape.size();
	if (N >= 2)
	{
		// Transform coordinates:
		mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
		for (int i = 0; i < N; i++)
		{
			origin.composePoint(
				robotShape[i].x, robotShape[i].y, 0, shap_x[i], shap_y[i],
				shap_z[i]);
		}

		gl_shape.appendLine(
			shap_x[0], shap_y[0], shap_z[0], shap_x[1], shap_y[1], shap_z[1]);
		for (int i = 0; i <= shap_x.size(); i++)
		{
			const int idx = i % shap_x.size();
			gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
		}
	}
}

void CPTG_RobotShape_Polygonal::add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines& gl_shape,
	const mrpt::poses::CPose2D& origin) const
{
	static_add_robotShape_to_setOfLines(gl_shape, origin, m_robotShape);
}

void CPTG_RobotShape_Polygonal::internal_shape_loadFromStream(
	mrpt::serialization::CArchive& in)
{
	uint8_t version;
	in >> version;

	switch (version)
	{
		case 0: in >> m_robotShape; break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	}
}

void CPTG_RobotShape_Polygonal::internal_shape_saveToStream(
	mrpt::serialization::CArchive& out) const
{
	uint8_t version = 0;
	out << version;

	out << m_robotShape;
}

double CPTG_RobotShape_Polygonal::getMaxRobotRadius() const
{
	return m_robotMaxRadius;
}

bool CPTG_RobotShape_Polygonal::isPointInsideRobotShape(
	const double x, const double y) const
{
	return m_robotShape.contains(mrpt::math::TPoint2D(x, y));
}

double CPTG_RobotShape_Polygonal::evalClearanceToRobotShape(
	const double ox, const double oy) const
{
	// Approximated computation, valid for relatively distant objects, which
	// is where clearance is useful.

	if (isPointInsideRobotShape(ox, oy)) return .0;

	double d = mrpt::hypot_fast(ox, oy) - m_robotMaxRadius;

	// if d<=0, we know from the isPointInsideRobotShape() above that
	// it's a false positive: enforce a minimum "fake" clearance:
	mrpt::keep_max(d, 0.1 * m_robotMaxRadius);

	return d;
}
