/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, 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/core/round.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/serialization/CArchive.h>

using namespace mrpt::nav;

CPTG_RobotShape_Circular::CPTG_RobotShape_Circular() = default;
CPTG_RobotShape_Circular::~CPTG_RobotShape_Circular() = default;
void CPTG_RobotShape_Circular::setRobotShapeRadius(const double robot_radius)
{
	m_robotRadius = robot_radius;
	internal_processNewRobotShape();
}

void CPTG_RobotShape_Circular::loadDefaultParams() { m_robotRadius = 0.2; }
void CPTG_RobotShape_Circular::loadShapeFromConfigFile(
	const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
{
	const double old_R = m_robotRadius;
	MRPT_LOAD_HERE_CONFIG_VAR(
		robot_radius, double, m_robotRadius, cfg, sSection);

	if (m_robotRadius != old_R) internal_processNewRobotShape();
}
void CPTG_RobotShape_Circular::saveToConfigFile(
	mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
{
	const int WN = 25, WV = 30;

	cfg.write(
		sSection, "robot_radius", m_robotRadius, WN, WV, "Robot radius [m].");
}

void CPTG_RobotShape_Circular::static_add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines& gl_shape, const mrpt::poses::CPose2D& origin,
	const double robotRadius)
{
	const double R = robotRadius;
	const int N = 17;
	// Transform coordinates:
	std::vector<double> shap_x(N), shap_y(N), shap_z(N);
	for (int i = 0; i < N; i++)
	{
		origin.composePoint(
			R * cos(i * 2 * M_PI / (N - 1)), R * sin(i * 2 * M_PI / (N - 1)), 0,
			shap_x[i], shap_y[i], shap_z[i]);
	}
	// Draw a "radius" to identify the "forward" orientation (phi=0)
	gl_shape.appendLine(
		origin.x(), origin.y(), .0, shap_x[0], shap_y[0], shap_z[0]);
	for (unsigned int i = 1; i <= shap_x.size(); i++)
	{
		const unsigned int idx = i % shap_x.size();
		gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
	}
	// Draw a "cross" to identify the robot center
	gl_shape.appendLine(
		origin.x() - R * 0.02, origin.y(), .0, origin.x() + R * 0.02,
		origin.y(), .0);
	gl_shape.appendLine(
		origin.x(), origin.y() - R * 0.02, .0, origin.x(),
		origin.y() + R * 0.02, .0);
}

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

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

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

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

	out << m_robotRadius;
}

double CPTG_RobotShape_Circular::getMaxRobotRadius() const
{
	return m_robotRadius;
}

bool CPTG_RobotShape_Circular::isPointInsideRobotShape(
	const double x, const double y) const
{
	return mrpt::hypot_fast(x, y) < m_robotRadius;
}

double CPTG_RobotShape_Circular::evalClearanceToRobotShape(
	const double ox, const double oy) const
{
	return mrpt::hypot_fast(ox, oy) - m_robotRadius;
}
