/* +---------------------------------------------------------------------------+
   |                     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 details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
 */

#include <mrpt/math/CMatrixD.h>
#include <mrpt/poses/CPoseInterpolatorBase.h>
//#include <mrpt/math/eigen_extensions.h>
#include <mrpt/math/slerp.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DPDFParticles.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/datetime.h>

#include <fstream>
#include <mrpt/math/interp_fit.hpp>

namespace mrpt::poses
{
template <int DIM>
CPoseInterpolatorBase<DIM>::CPoseInterpolatorBase()
	: maxTimeInterpolation(std::chrono::seconds(-1))

{
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::clear()
{
	m_path.clear();
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::insert(
	const mrpt::Clock::time_point& t, const cpose_t& p)
{
	m_path[t] = p.asTPose();
}
template <int DIM>
void CPoseInterpolatorBase<DIM>::insert(
	const mrpt::Clock::time_point& t, const pose_t& p)
{
	m_path[t] = p;
}

/*---------------------------------------------------------------
						interpolate
  ---------------------------------------------------------------*/
template <int DIM>
typename CPoseInterpolatorBase<DIM>::cpose_t&
	CPoseInterpolatorBase<DIM>::interpolate(
		const mrpt::Clock::time_point& t, cpose_t& out_interp,
		bool& out_valid_interp) const
{
	pose_t p;
	this->interpolate(t, p, out_valid_interp);
	out_interp = cpose_t(p);
	return out_interp;
}

template <int DIM>
typename CPoseInterpolatorBase<DIM>::pose_t&
	CPoseInterpolatorBase<DIM>::interpolate(
		const mrpt::Clock::time_point& t, pose_t& out_interp,
		bool& out_valid_interp) const
{
	// Default value in case of invalid interp
	for (size_t k = 0; k < pose_t::static_size; k++)
	{
		out_interp[k] = 0;
	}
	TTimePosePair p1, p2, p3, p4;
	p1.second = p2.second = p3.second = p4.second = out_interp;

	// We'll look for 4 consecutive time points.
	// Check if the selected method needs all 4 points or just the central 2 of
	// them:
	bool interp_method_requires_4pts;
	switch (m_method)
	{
		case imLinear2Neig:
		case imSplineSlerp:
		case imLinearSlerp: interp_method_requires_4pts = false; break;
		default: interp_method_requires_4pts = true; break;
	};

	// Out of range?
	auto it_ge1 = m_path.lower_bound(t);

	// Exact match?
	if (it_ge1 != m_path.end() && it_ge1->first == t)
	{
		out_interp = it_ge1->second;
		out_valid_interp = true;
		return out_interp;
	}

	// Are we in the beginning or the end of the path?
	if (it_ge1 == m_path.end() || it_ge1 == m_path.begin())
	{
		out_valid_interp = false;
		return out_interp;
	}  // end

	p3 = *it_ge1;  // Third pair
	auto it_ge2 = it_ge1;
	++it_ge2;
	if (it_ge2 == m_path.end())
	{
		if (interp_method_requires_4pts)
		{
			out_valid_interp = false;
			return out_interp;
		}
	}
	else
	{
		p4 = *(it_ge2);	 // Fourth pair
	}

	p2 = *(--it_ge1);  // Second pair

	if (it_ge1 == m_path.begin())
	{
		if (interp_method_requires_4pts)
		{
			out_valid_interp = false;
			return out_interp;
		}
	}
	else
	{
		p1 = *(--it_ge1);  // First pair
	}

	// Test if the difference between the desired timestamp and the next
	// timestamp is lower than a certain (configurable) value
	const mrpt::Clock::duration dt12 = interp_method_requires_4pts
		? (p2.first - p1.first)
		: mrpt::Clock::duration(0);
	const mrpt::Clock::duration dt23 = (p3.first - p2.first);
	const mrpt::Clock::duration dt34 = interp_method_requires_4pts
		? (p4.first - p3.first)
		: mrpt::Clock::duration(0);

	if (maxTimeInterpolation.count() > 0 &&
		(dt12 > maxTimeInterpolation || dt23 > maxTimeInterpolation ||
		 dt34 > maxTimeInterpolation))
	{
		out_valid_interp = false;
		return out_interp;
	}

	// Do interpolation:
	// ------------------------------------------
	// First Previous point:  p1
	// Second Previous point: p2
	// First Next point:	  p3
	// Second Next point:     p4
	// Time where to interpolate:  t

	impl_interpolation(p1, p2, p3, p4, m_method, t, out_interp);

	out_valid_interp = true;
	return out_interp;

}  // end interpolate

template <int DIM>
bool CPoseInterpolatorBase<DIM>::getPreviousPoseWithMinDistance(
	const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose)
{
	pose_t p;
	bool ret = getPreviousPoseWithMinDistance(t, distance, p);
	out_pose = cpose_t(p);
	return ret;
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::getPreviousPoseWithMinDistance(
	const mrpt::Clock::time_point& t, double distance, pose_t& out_pose)
{
	if (m_path.size() == 0 || distance <= 0) return false;

	pose_t myPose;

	// Search for the desired timestamp
	auto it = m_path.find(t);
	if (it != m_path.end() && it != m_path.begin()) myPose = it->second;
	else
		return false;

	double d = 0.0;
	do
	{
		--it;
		d = (point_t(myPose) - point_t(it->second)).norm();
	} while (d < distance && it != m_path.begin());

	if (d >= distance)
	{
		out_pose = it->second;
		return true;
	}
	else
		return false;
}  // end getPreviousPose

template <int DIM>
void CPoseInterpolatorBase<DIM>::setMaxTimeInterpolation(
	const mrpt::Clock::duration& time)
{
	ASSERT_(time.count() > 0);
	maxTimeInterpolation = time;
}

template <int DIM>
mrpt::Clock::duration CPoseInterpolatorBase<DIM>::getMaxTimeInterpolation()
{
	return maxTimeInterpolation;
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveToTextFile(const std::string& s) const
{
	try
	{
		std::ofstream f;
		f.open(s);
		if (!f.is_open()) return false;
		std::string str;
		for (auto i = m_path.begin(); i != m_path.end(); ++i)
		{
			const double t = mrpt::system::timestampTotime_t(i->first);
			const auto& p = i->second;

			str = mrpt::format("%.06f ", t);
			for (unsigned int k = 0; k < p.size(); k++)
				str += mrpt::format("%.06f ", p[k]);
			str += std::string("\n");

			f << str;
		}
		return true;
	}
	catch (...)
	{
		return false;
	}
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveToTextFile_TUM(const std::string& s) const
{
	try
	{
		std::ofstream f;
		f.open(s);
		if (!f.is_open()) return false;
		std::string str;
		for (auto i = m_path.begin(); i != m_path.end(); ++i)
		{
			const double t = mrpt::system::timestampTotime_t(i->first);
			const auto p =
				mrpt::poses::CPose3DQuat(mrpt::poses::CPose3D(i->second));

			// TUM format: timestamp x y z q_x q_y q_z q_w
			str = mrpt::format("%.06f", t);
			constexpr std::array<int, 7> idxs = {0, 1, 2, 4, 5, 6, 3};
			for (unsigned int k = 0; k < 7; k++)
				str += mrpt::format(" %.06f", p[idxs[k]]);
			str += std::string("\n");
			f << str;
		}
		return true;
	}
	catch (...)
	{
		return false;
	}
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::saveInterpolatedToTextFile(
	const std::string& s, const mrpt::Clock::duration& period) const
{
	using mrpt::Clock;
	try
	{
		std::ofstream f;
		f.open(s);
		if (!f.is_open()) return false;
		if (m_path.empty()) return true;

		std::string str;

		const Clock::time_point t_ini = m_path.begin()->first;
		const Clock::time_point t_end = m_path.rbegin()->first;

		pose_t p;
		bool valid;
		for (Clock::time_point t = t_ini; t <= t_end; t += period)
		{
			this->interpolate(t, p, valid);
			if (!valid) continue;

			str = mrpt::format("%.06f ", mrpt::system::timestampTotime_t(t));
			for (unsigned int k = 0; k < p.size(); k++)
				str += mrpt::format("%.06f ", p[k]);
			str += std::string("\n");
			f << str;
		}
		return true;
	}
	catch (...)
	{
		return false;
	}
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::loadFromTextFile(const std::string& s)
{
	MRPT_START

	clear();
	mrpt::math::CMatrixD M;

	try
	{
		M.loadFromTextFile(s);
	}
	catch (std::exception&)
	{
		return false;  // error loading file
	}

	// Check valid format:
	if (M.rows() == 0) return false;
	ASSERT_(M.cols() == pose_t::static_size + 1);

	// load into the path:
	const size_t N = M.rows();
	pose_t p;
	for (size_t i = 0; i < N; i++)
	{
		for (unsigned int k = 0; k < pose_t::static_size; k++)
			p[k] = M(i, k + 1);
		insert(mrpt::system::time_tToTimestamp(M(i, 0)), p);
	}
	return true;
	MRPT_END
}

template <int DIM>
bool CPoseInterpolatorBase<DIM>::loadFromTextFile_TUM(const std::string& s)
{
	MRPT_START

	clear();
	mrpt::math::CMatrixD M;

	try
	{
		M.loadFromTextFile(s);
	}
	catch (std::exception&)
	{
		return false;  // error loading file
	}

	// Check valid format:
	if (M.rows() == 0) return false;
	ASSERT_(M.cols() == 3 + 4 + 1);

	// load into the path:
	const size_t N = M.rows();
	constexpr std::array<int, 7> idxs = {0, 1, 2, 4, 5, 6, 3};
	mrpt::poses::CPose3DQuat p;
	for (size_t i = 0; i < N; i++)
	{
		for (unsigned int k = 0; k < 7; k++)
			p[idxs[k]] = M(i, k + 1);
		insert(
			mrpt::system::time_tToTimestamp(M(i, 0)),
			pose_t(mrpt::poses::CPose3D(p).asTPose()));
	}
	return true;
	MRPT_END
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::getBoundingBox(
	point_t& Min, point_t& Max) const
{
	MRPT_START
	ASSERT_(!m_path.empty());

	for (unsigned int k = 0; k < point_t::static_size; k++)
	{
		Min[k] = std::numeric_limits<double>::max();
		Max[k] = -std::numeric_limits<double>::max();
	}

	for (auto p = m_path.begin(); p != m_path.end(); ++p)
	{
		for (unsigned int k = 0; k < point_t::static_size; k++)
		{
			mrpt::keep_min(Min[k], p->second[k]);
			mrpt::keep_max(Max[k], p->second[k]);
		}
	}
	MRPT_END
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::setInterpolationMethod(
	mrpt::poses::TInterpolatorMethod method)
{
	m_method = method;
}

template <int DIM>
TInterpolatorMethod CPoseInterpolatorBase<DIM>::getInterpolationMethod() const
{
	return m_method;
}

template <int DIM>
void CPoseInterpolatorBase<DIM>::filter(
	unsigned int component, unsigned int samples)
{
	if (m_path.empty()) return;

	TPath aux;

	int ant, post;
	size_t nitems = size();

	post = (samples % 2) ? (unsigned int)(samples / 2) : samples / 2;
	ant = (unsigned int)(samples / 2);

	int k = 0;
	iterator it1, it2, it3;

	// int asamples;
	for (it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k)
	{
		it2 = m_path.begin();
		if (k - ant > 0) advance(it2, k - ant);

		if (k + post < (int)nitems)
		{
			it3 = m_path.begin();
			advance(it3, k + post + 1);
		}
		else
		{
			it3 = m_path.end();
		}

		unsigned int nsamples = distance(it2, it3);
		CPose3DPDFParticles particles(nsamples);
		for (unsigned int i = 0; it2 != it3; ++it2, ++i)
		{
			particles.m_particles[i].log_w = 0;
			particles.m_particles[i].d = it1->second;
			switch (component)
			{
				case 0: particles.m_particles[i].d.x = it2->second[0]; break;
				case 1: particles.m_particles[i].d.y = it2->second[1]; break;
				case 2: particles.m_particles[i].d.z = it2->second[2]; break;
				case 3: particles.m_particles[i].d.yaw = it2->second[3]; break;
				case 4:
					particles.m_particles[i].d.pitch = it2->second[4];
					break;
				case 5: particles.m_particles[i].d.roll = it2->second[5]; break;
			}  // end switch
		}  // end for it2

		mrpt::poses::CPose3D auxPose;
		particles.getMean(auxPose);
		aux[it1->first] = pose_t(auxPose.asTPose());
	}  // end for it1
	m_path = aux;
}
}  // namespace mrpt::poses
