/* +------------------------------------------------------------------------+
   |                     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/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/metaprogramming_serialization.h>
#include <mrpt/system/os.h>

#include <iterator>

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

IMPLEMENTS_SERIALIZABLE(CSensoryFrame, CSerializable, mrpt::obs)

void CSensoryFrame::clear()
{
	m_observations.clear();
	m_cachedMap.reset();
}

uint8_t CSensoryFrame::serializeGetVersion() const { return 2; }
void CSensoryFrame::serializeTo(mrpt::serialization::CArchive& out) const
{
	out.WriteAs<uint32_t>(m_observations.size());
	for (const auto& o : m_observations)
	{
		ASSERT_(o);
		out << *o;
	}
}

void CSensoryFrame::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	MRPT_START
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		{
			uint32_t i, n;
			mrpt::system::TTimeStamp tempTimeStamp = INVALID_TIMESTAMP;

			clear();
			if (version < 2)  // ID was removed in version 2
			{
				uint32_t ID;
				in >> ID;
			}

			if (version == 0) in.ReadBufferFixEndianness(&tempTimeStamp, 1);

			in >> n;
			m_observations.resize(n);
			for_each(
				m_observations.begin(), m_observations.end(),
				mrpt::serialization::metaprogramming::ObjectReadFromStream(
					&in));

			if (version == 0)
				for (i = 0; i < n; i++)
					m_observations[i]->timestamp = tempTimeStamp;
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};

	m_cachedMap.reset();

	MRPT_END
}

void CSensoryFrame::operator+=(const CSensoryFrame& sf)
{
	m_cachedMap.reset();
	for (const auto& obs : sf)
		m_observations.push_back(obs);
}

/*---------------------------------------------------------------
						operator +=
  ---------------------------------------------------------------*/
void CSensoryFrame::operator+=(const CObservation::Ptr& obs)
{
	m_cachedMap.reset();
	m_observations.push_back(obs);
}

void CSensoryFrame::push_back(const CObservation::Ptr& obs)
{
	m_cachedMap.reset();
	m_observations.push_back(obs);
}

/*---------------------------------------------------------------
				eraseByIndex
  ---------------------------------------------------------------*/
void CSensoryFrame::eraseByIndex(size_t idx)
{
	MRPT_START
	if (idx >= size())
		THROW_EXCEPTION_FMT(
			"Index %u out of range.", static_cast<unsigned>(idx));

	m_cachedMap.reset();
	auto it = begin() + idx;
	ASSERT_(!*it);
	m_observations.erase(it);
	MRPT_END
}

/*---------------------------------------------------------------
					getObservationByIndex
  ---------------------------------------------------------------*/
const CObservation::Ptr& CSensoryFrame::getObservationByIndex(size_t idx) const
{
	MRPT_START
	ASSERT_LT_(idx, size());
	auto it = begin() + idx;
	return *it;
	MRPT_END
}
CObservation::Ptr& CSensoryFrame::getObservationByIndex(size_t idx)
{
	MRPT_START
	ASSERT_LT_(idx, size());
	auto it = begin() + idx;
	return *it;
	MRPT_END
}

/*---------------------------------------------------------------
					erase
  ---------------------------------------------------------------*/
CSensoryFrame::iterator CSensoryFrame::erase(const iterator& it)
{
	MRPT_START
	ASSERT_(it != end());
	m_cachedMap.reset();

	return m_observations.erase(it);
	MRPT_END
}

/*---------------------------------------------------------------
					getObservationBySensorLabel
  ---------------------------------------------------------------*/
CObservation::Ptr CSensoryFrame::getObservationBySensorLabel(
	const std::string& label, size_t idx) const
{
	MRPT_START

	size_t foundCount = 0;
	for (const auto& it : *this)
		if (!os::_strcmpi(it->sensorLabel.c_str(), label.c_str()))
			if (foundCount++ == idx) return it;

	return CObservation::Ptr();

	MRPT_END
}

/*---------------------------------------------------------------
						swap
  ---------------------------------------------------------------*/
void CSensoryFrame::swap(CSensoryFrame& sf)
{
	m_observations.swap(sf.m_observations);
	std::swap(m_cachedMap, sf.m_cachedMap);
}

/*---------------------------------------------------------------
						eraseByLabel
  ---------------------------------------------------------------*/
void CSensoryFrame::eraseByLabel(const std::string& label)
{
	for (auto it = begin(); it != end();)
	{
		if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
		{ it = erase(it); }
		else
			it++;
	}
	m_cachedMap.reset();
}

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;	// impl in
// CObservation2DRangeScan.cpp
}  // namespace mrpt::obs
/*---------------------------------------------------------------
						internal_buildAuxPointsMap
  ---------------------------------------------------------------*/
void CSensoryFrame::internal_buildAuxPointsMap(const void* options) const
{
	if (!ptr_internal_build_points_map_from_scan2D)
		throw std::runtime_error(
			"[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs "
			"linking against mrpt-maps.\n");

	for (const auto& it : *this)
		if (IS_CLASS(*it, CObservation2DRangeScan))
			(*ptr_internal_build_points_map_from_scan2D)(
				dynamic_cast<CObservation2DRangeScan&>(*it.get()), m_cachedMap,
				options);
}

bool CSensoryFrame::insertObservationsInto(
	maps::CMetricMap& theMap,
	const std::optional<const mrpt::poses::CPose3D>& robotPose) const
{
	bool anyone = false;
	for (const auto& it : *this)
		anyone |= it->insertObservationInto(theMap, robotPose);
	return anyone;
}
