/* +------------------------------------------------------------------------+
   |                     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 "obs-precomp.h"  // Precompiled headers
//
#include <mrpt/core/round.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/serialization/CArchive.h>

#if MRPT_HAS_MATLAB
#include <mexplus.h>
#endif

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

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CObservation2DRangeScan, CObservation, mrpt::obs)

uint8_t CObservation2DRangeScan::serializeGetVersion() const { return 7; }
void CObservation2DRangeScan::serializeTo(
	mrpt::serialization::CArchive& out) const
{
	// The data
	out << aperture << rightToLeft << maxRange << sensorPose;
	uint32_t N = m_scan.size();
	out << N;
	ASSERT_EQUAL_(m_validRange.size(), m_scan.size());
	if (N)
	{
		out.WriteBufferFixEndianness(&m_scan[0], N);
		out.WriteBuffer(&m_validRange[0], sizeof(m_validRange[0]) * N);
	}
	out << stdError;
	out << timestamp;
	out << beamAperture;

	out << sensorLabel;

	out << deltaPitch;

	out << hasIntensity();
	if (hasIntensity()) out.WriteBufferFixEndianness(&m_intensity[0], N);
}

void CObservation2DRangeScan::truncateByDistanceAndAngle(
	float min_distance, float max_angle, float min_height, float max_height,
	float h)
{
	// FILTER OUT INVALID POINTS!!
	CPose3D pose;
	unsigned int k = 0;
	const auto nPts = m_scan.size();

	auto itValid = m_validRange.begin();
	for (auto itScan = m_scan.begin(); itScan != m_scan.end();
		 itScan++, itValid++, k++)
	{
		const auto ang = std::abs(k * aperture / nPts - aperture * 0.5f);
		float x = (*itScan) * cos(ang);

		if (min_height != 0 || max_height != 0)
		{
			ASSERT_(max_height > min_height);
			if (*itScan < min_distance || ang > max_angle ||
				x > h - min_height || x < h - max_height)
				*itValid = false;
		}  // end if
		else if (*itScan < min_distance || ang > max_angle)
			*itValid = false;
	}
}

void CObservation2DRangeScan::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		{
			CMatrixF covSensorPose;
			in >> aperture >> rightToLeft >> maxRange >> sensorPose >>
				covSensorPose;
			uint32_t N, i;

			in >> N;

			this->resizeScan(N);
			if (N) in.ReadBufferFixEndianness(&m_scan[0], N);

			if (version >= 1)
			{  // Load validRange:
				if (N)
					in.ReadBuffer(
						&m_validRange[0], sizeof(m_validRange[0]) * N);
			}
			else
			{
				// validRange: Default values: If distance is not maxRange
				for (i = 0; i < N; i++)
					m_validRange[i] = m_scan[i] < maxRange;
			}

			if (version >= 2) { in >> stdError; }
			else
			{
				stdError = 0.01f;
			}

			if (version >= 3) { in >> timestamp; }

			// Default values for newer versions:
			beamAperture = DEG2RAD(0.25f);

			deltaPitch = 0;
			sensorLabel = "";
		}
		break;

		case 4:
		case 5:
		case 6:
		case 7:
		{
			uint32_t N;

			CMatrixF covSensorPose;
			in >> aperture >> rightToLeft >> maxRange >> sensorPose;

			if (version < 6)  // covSensorPose was removed in version 6
				in >> covSensorPose;

			in >> N;
			this->resizeScan(N);
			if (N)
			{
				in.ReadBufferFixEndianness(&m_scan[0], N);
				in.ReadBuffer(&m_validRange[0], sizeof(m_validRange[0]) * N);
			}
			in >> stdError;
			in.ReadBufferFixEndianness(&timestamp, 1);
			in >> beamAperture;

			if (version >= 5)
			{
				in >> sensorLabel;
				in >> deltaPitch;
			}
			else
			{
				sensorLabel = "";
				deltaPitch = 0;
			}
			if (version >= 7)
			{
				bool hasIntensity;
				in >> hasIntensity;
				setScanHasIntensity(hasIntensity);
				if (hasIntensity && N)
				{ in.ReadBufferFixEndianness(&m_intensity[0], N); }
			}
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};

	m_cachedMap.reset();
}

/*---------------------------------------------------------------
  Implements the writing to a mxArray for Matlab
 ---------------------------------------------------------------*/
#if MRPT_HAS_MATLAB
// Add to implement mexplus::from template specialization
IMPLEMENTS_MEXPLUS_FROM(mrpt::obs::CObservation2DRangeScan)
#endif

mxArray* CObservation2DRangeScan::writeToMatlab() const
{
#if MRPT_HAS_MATLAB
	const char* fields[] = {
		"class",  // Data common to any MRPT class
		"ts",
		"sensorLabel",	// Data common to any observation
		"scan",
		"validRange",
		"intensity"	 // Received raw data
		"aperture",
		"rightToLeft",
		"maxRange",	 // Scan plane geometry and properties
		"stdError",
		"beamAperture",
		"deltaPitch",  // Ray properties
		"pose",	 // Sensor pose
		"map"};	 // Points map
	mexplus::MxArray obs_struct(
		mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));

	obs_struct.set("class", this->GetRuntimeClass()->className);

	obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
	obs_struct.set("sensorLabel", this->sensorLabel);

	obs_struct.set("scan", this->m_scan);
	// TODO: "validRange should be a vector<bool> for Matlab instead of
	// vector<char>" ==> JLBC: It cannot be done like that since serializing
	// "bool" is not cross-platform safe, while "char" is...
	obs_struct.set("validRange", this->m_validRange);
	obs_struct.set("intensity", this->m_intensity);
	obs_struct.set("aperture", this->aperture);
	obs_struct.set("rightToLeft", this->rightToLeft);
	obs_struct.set("maxRange", this->maxRange);
	obs_struct.set("stdError", this->stdError);
	obs_struct.set("beamAperture", this->beamAperture);
	obs_struct.set("deltaPitch", this->deltaPitch);
	obs_struct.set("pose", this->sensorPose);
	// TODO: obs_struct.set("map", ...)
	return obs_struct.release();
#else
	THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
#endif
}

/*---------------------------------------------------------------
						isPlanarScan
 ---------------------------------------------------------------*/
bool CObservation2DRangeScan::isPlanarScan(const double tolerance) const
{
	return sensorPose.isHorizontal(tolerance);
}

bool CObservation2DRangeScan::hasIntensity() const { return m_has_intensity; }
void CObservation2DRangeScan::setScanHasIntensity(bool setHasIntensityFlag)
{
	m_has_intensity = setHasIntensityFlag;
}

/*---------------------------------------------------------------
						filterByExclusionAreas
 ---------------------------------------------------------------*/
void CObservation2DRangeScan::filterByExclusionAreas(
	const TListExclusionAreasWithRanges& areas)
{
	if (areas.empty()) return;

	MRPT_START

	float Ang, dA;
	const size_t sizeRangeScan = m_scan.size();

	ASSERT_EQUAL_(m_scan.size(), m_validRange.size());

	if (!sizeRangeScan) return;

	if (rightToLeft)
	{
		Ang = -0.5f * aperture;
		dA = aperture / (sizeRangeScan - 1);
	}
	else
	{
		Ang = +0.5f * aperture;
		dA = -aperture / (sizeRangeScan - 1);
	}

	auto valid_it = m_validRange.begin();
	for (auto scan_it = m_scan.begin(); scan_it != m_scan.end();
		 scan_it++, valid_it++)
	{
		if (!*valid_it)
		{
			Ang += dA;
			continue;  // Already it's invalid
		}

		// Compute point in 2D, local to the laser center:
		const auto Lx = *scan_it * cos(Ang);
		const auto Ly = *scan_it * sin(Ang);
		Ang += dA;

		// To real 3D pose:
		double Gx, Gy, Gz;
		this->sensorPose.composePoint(Lx, Ly, 0, Gx, Gy, Gz);

		// Filter by X,Y:
		for (const auto& area : areas)
		{
			if (area.first.PointIntoPolygon(Gx, Gy) &&
				(Gz >= area.second.first && Gz <= area.second.second))
			{
				*valid_it = false;
				break;	// Go for next point
			}
		}  // for each area
	}  // for each point

	MRPT_END
}

float CObservation2DRangeScan::getScanAngle(const size_t idx) const
{
	float Ang = -0.5f * aperture, dA = aperture / (m_scan.size() - 1);
	ASSERT_BELOW_(idx, m_scan.size());

	if (!rightToLeft)
	{
		Ang = -Ang;
		dA = -dA;
	}
	return Ang + dA * idx;
}

/*---------------------------------------------------------------
						filterByExclusionAreas
 ---------------------------------------------------------------*/
void CObservation2DRangeScan::filterByExclusionAreas(
	const std::vector<mrpt::math::CPolygon>& areas)
{
	if (areas.empty()) return;

	TListExclusionAreasWithRanges lst;
	for (const auto& area : areas)
	{
		TListExclusionAreasWithRanges::value_type dat;
		dat.first = area;
		dat.second.first = -std::numeric_limits<double>::max();
		dat.second.second = std::numeric_limits<double>::max();

		lst.push_back(dat);
	}
	filterByExclusionAreas(lst);
}

/*---------------------------------------------------------------
						filterByExclusionAngles
 ---------------------------------------------------------------*/
void CObservation2DRangeScan::filterByExclusionAngles(
	const std::vector<std::pair<double, double>>& angles)
{
	if (angles.empty()) return;

	MRPT_START

	double Ang, dA;
	const size_t sizeRangeScan = m_scan.size();

	ASSERT_EQUAL_(m_scan.size(), m_validRange.size());

	if (!sizeRangeScan) return;

	if (rightToLeft)
	{
		Ang = -0.5 * aperture;
		dA = aperture / (sizeRangeScan - 1);
	}
	else
	{
		Ang = +0.5 * aperture;
		dA = -aperture / (sizeRangeScan - 1);
	}

	// For each forbiden angle range:
	for (const auto& angle : angles)
	{
		int ap_idx_ini =
			mrpt::round(mrpt::math::wrapTo2Pi(angle.first - Ang) / dA);
		int ap_idx_end =
			mrpt::round(mrpt::math::wrapTo2Pi(angle.second - Ang) / dA);

		if (ap_idx_ini < 0) ap_idx_ini = 0;
		if (ap_idx_end < 0) ap_idx_end = 0;

		if (ap_idx_ini > static_cast<int>(sizeRangeScan))
			ap_idx_ini = sizeRangeScan - 1;
		if (ap_idx_end > static_cast<int>(sizeRangeScan))
			ap_idx_end = sizeRangeScan - 1;

		const size_t idx_ini = ap_idx_ini;
		const size_t idx_end = ap_idx_end;

		if (idx_end >= idx_ini)
		{
			for (size_t i = idx_ini; i <= idx_end; i++)
				m_validRange[i] = false;
		}
		else
		{
			for (size_t i = 0; i < idx_end; i++)
				m_validRange[i] = false;

			for (size_t i = idx_ini; i < sizeRangeScan; i++)
				m_validRange[i] = false;
		}
	}

	MRPT_END
}

namespace mrpt::obs
{
// Tricky way to call to a library that depends on us, a sort of "run-time"
// linking: ptr_internal_build_points_map_from_scan2D is a functor in
// "mrpt-obs", set by "mrpt-maps" at its startup.
using scan2pts_functor = void (*)(
	const mrpt::obs::CObservation2DRangeScan& obs,
	mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);

extern scan2pts_functor ptr_internal_build_points_map_from_scan2D;
scan2pts_functor ptr_internal_build_points_map_from_scan2D = nullptr;

void internal_set_build_points_map_from_scan2D(scan2pts_functor fn)
{
	ptr_internal_build_points_map_from_scan2D = fn;
}
}  // namespace mrpt::obs

/*---------------------------------------------------------------
						internal_buildAuxPointsMap
  ---------------------------------------------------------------*/
void CObservation2DRangeScan::internal_buildAuxPointsMap(
	const void* options) const
{
	if (!ptr_internal_build_points_map_from_scan2D)
		throw std::runtime_error(
			"[CObservation2DRangeScan::buildAuxPointsMap] ERROR: This function "
			"needs linking against mrpt-maps.\n");

	(*ptr_internal_build_points_map_from_scan2D)(*this, m_cachedMap, options);
}

/** Fill out a T2DScanProperties structure with the parameters of this scan */
void CObservation2DRangeScan::getScanProperties(T2DScanProperties& p) const
{
	p.nRays = m_scan.size();
	p.aperture = this->aperture;
	p.rightToLeft = this->rightToLeft;
}

bool mrpt::obs::operator<(
	const T2DScanProperties& a, const T2DScanProperties& b)
{
	if (a.nRays != b.nRays) return a.nRays < b.nRays;
	if (a.aperture != b.aperture) return a.aperture < b.aperture;
	if (a.rightToLeft != b.rightToLeft) return a.rightToLeft;
	return false;
}

void CObservation2DRangeScan::getDescriptionAsText(std::ostream& o) const
{
	CObservation::getDescriptionAsText(o);
	o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
		 "base:\n";
	o << sensorPose.getHomogeneousMatrixVal<CMatrixDouble44>() << "\n"
	  << sensorPose << "\n";

	o << format(
		"Samples direction: %s\n",
		(rightToLeft) ? "Right->Left" : "Left->Right");
	o << "Points in the scan: " << m_scan.size() << "\n";
	o << format("Estimated sensor 'sigma': %f\n", stdError);
	o << format(
		"Increment in pitch during the scan: %f deg\n", RAD2DEG(deltaPitch));

	size_t i, inval = 0;
	for (i = 0; i < m_scan.size(); i++)
		if (!m_validRange[i]) inval++;
	o << format("Invalid points in the scan: %u\n", (unsigned)inval);

	o << format("Sensor maximum range: %.02f m\n", maxRange);
	o << format(
		"Sensor field-of-view (\"aperture\"): %.01f deg\n", RAD2DEG(aperture));

	o << "Raw scan values: [";
	for (i = 0; i < m_scan.size(); i++)
		o << format("%.03f ", m_scan[i]);
	o << "]\n";

	o << "Raw valid-scan values: [";
	for (i = 0; i < m_validRange.size(); i++)
		o << format("%u ", m_validRange[i] ? 1 : 0);
	o << "]\n\n";

	if (hasIntensity())
	{
		o << "Raw intensity values: [";
		for (i = 0; i < m_intensity.size(); i++)
			o << format("%d ", m_intensity[i]);
		o << "]\n\n";
	}
}

const float& CObservation2DRangeScan::getScanRange(const size_t i) const
{
	ASSERT_LT_(i, m_scan.size());
	return m_scan[i];
}
float& CObservation2DRangeScan::getScanRange(const size_t i)
{
	ASSERT_LT_(i, m_scan.size());
	return m_scan[i];
}

void CObservation2DRangeScan::setScanRange(const size_t i, const float val)
{
	ASSERT_LT_(i, m_scan.size());
	m_scan[i] = val;
}

const int32_t& CObservation2DRangeScan::getScanIntensity(const size_t i) const
{
	ASSERT_LT_(i, m_intensity.size());
	return m_intensity[i];
}
int32_t& CObservation2DRangeScan::getScanIntensity(const size_t i)
{
	ASSERT_LT_(i, m_intensity.size());
	return m_intensity[i];
}
void CObservation2DRangeScan::setScanIntensity(const size_t i, const int val)
{
	ASSERT_LT_(i, m_intensity.size());
	m_intensity[i] = val;
}

bool CObservation2DRangeScan::getScanRangeValidity(const size_t i) const
{
	ASSERT_LT_(i, m_validRange.size());
	return m_validRange[i] != 0;
}
void CObservation2DRangeScan::setScanRangeValidity(
	const size_t i, const bool val)
{
	ASSERT_LT_(i, m_validRange.size());
	m_validRange[i] = val ? 1 : 0;
}

void CObservation2DRangeScan::resizeScan(const size_t len)
{
	m_scan.resize(len);
	m_intensity.resize(len);
	m_validRange.resize(len);
}

void CObservation2DRangeScan::resizeScanAndAssign(
	const size_t len, const float rangeVal, const bool rangeValidity,
	const int32_t rangeIntensity)
{
	m_scan.assign(len, rangeVal);
	m_validRange.assign(len, rangeValidity);
	m_intensity.assign(len, rangeIntensity);
}

size_t CObservation2DRangeScan::getScanSize() const { return m_scan.size(); }
void CObservation2DRangeScan::loadFromVectors(
	size_t nRays, const float* scanRanges, const char* scanValidity)
{
	ASSERT_(scanRanges);
	ASSERT_(scanValidity);
	resizeScan(nRays);
	for (size_t i = 0; i < nRays; i++)
	{
		m_scan[i] = scanRanges[i];
		m_validRange[i] = scanValidity[i];
	}
}

// See base class docs:
std::string CObservation2DRangeScan::exportTxtHeader() const
{
	std::string ret = "RANGES[i] ... VALID[i]";
	if (hasIntensity()) ret += " ... INTENSITY[i]";
	return ret;
}

std::string CObservation2DRangeScan::exportTxtDataRow() const
{
	std::stringstream o;
	for (size_t i = 0; i < m_scan.size(); i++)
		o << format("%.03f ", m_scan[i]);
	o << "    ";

	for (size_t i = 0; i < m_validRange.size(); i++)
		o << format("%u ", m_validRange[i] ? 1 : 0);
	o << "    ";

	if (hasIntensity())
	{
		for (size_t i = 0; i < m_intensity.size(); i++)
			o << format("%d ", m_intensity[i]);
	}
	return o.str();
}
