/* +------------------------------------------------------------------------+
   |                     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 "maps-precomp.h"  // Precomp header
//
#include <mrpt/maps/COccupancyGridMap3D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/poses/CPose3D.h>

using namespace mrpt::maps;

// bits to left-shift for fixed-point arithmetic simulation in raytracing.
static constexpr unsigned FRBITS = 9;

bool COccupancyGridMap3D::internal_insertObservation(
	const mrpt::obs::CObservation& obs,
	const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
	MRPT_START

	const mrpt::poses::CPose3D robotPose3D =
		(robotPose) ? *robotPose : mrpt::poses::CPose3D();

	if (auto* o = dynamic_cast<const mrpt::obs::CObservation2DRangeScan*>(&obs);
		o != nullptr)
	{
		this->internal_insertObservationScan2D(*o, robotPose3D);
		return true;
	}
	if (auto* o = dynamic_cast<const mrpt::obs::CObservation3DRangeScan*>(&obs);
		o != nullptr)
	{
		this->internal_insertObservationScan3D(*o, robotPose3D);
		return true;
	}
	if (auto* o = dynamic_cast<const mrpt::obs::CObservationPointCloud*>(&obs);
		o != nullptr)
	{
		this->internal_insertObservationPointCloud(*o, robotPose3D);
		return true;
	}

	return false;

	MRPT_END
}

void COccupancyGridMap3D::internal_insertObservationScan2D(
	const mrpt::obs::CObservation2DRangeScan& o,
	const mrpt::poses::CPose3D& robotPose)
{
	MRPT_START

	// Convert to point cloud:
	mrpt::maps::CSimplePointsMap pts;
	pts.insertionOptions.also_interpolate = false;
	pts.insertionOptions.addToExistingPointsMap = true;
	pts.insertionOptions.disableDeletion = true;
	pts.insertionOptions.fuseWithExisting = false;
	pts.insertionOptions.insertInvalidPoints = false;
	pts.insertionOptions.minDistBetweenLaserPoints = .0;  // insert all

	pts.loadFromRangeScan(o, robotPose);

	const auto sensorPose3D = robotPose + o.sensorPose;
	const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose());
	insertPointCloud(sensorPt, pts);

	MRPT_END
}

void COccupancyGridMap3D::insertPointCloud(
	const mrpt::math::TPoint3D& sensorPt, const mrpt::maps::CPointsMap& pts,
	const float maxValidRange, const std::optional<poses::CPose3D>& robotPose)
{
	MRPT_START

	const auto& xs = pts.getPointsBufferRef_x();
	const auto& ys = pts.getPointsBufferRef_y();
	const auto& zs = pts.getPointsBufferRef_z();

	// Process points one by one as rays:
	for (std::size_t idx = 0; idx < xs.size();
		 idx += insertionOptions.decimation)
	{
		auto pt = mrpt::math::TPoint3D(xs[idx], ys[idx], zs[idx]);
		if (robotPose) pt = robotPose->composePoint(pt);

		insertRay(sensorPt, pt);
	}

	MRPT_END
}

void COccupancyGridMap3D::internal_insertObservationScan3D(
	const mrpt::obs::CObservation3DRangeScan& o,
	const mrpt::poses::CPose3D& robotPose)
{
	MRPT_START

	// Depth -> 3D points:
	mrpt::maps::CSimplePointsMap pts;
	mrpt::obs::T3DPointsProjectionParams pp;
	pp.takeIntoAccountSensorPoseOnRobot = false;  // done below
	pp.decimation = insertionOptions.decimation_3d_range;

	const_cast<mrpt::obs::CObservation3DRangeScan&>(o).unprojectInto(pts, pp);

	const auto sensorPose3D = robotPose + o.sensorPose;
	// Shift everything to its proper pose in the global frame:
	pts.changeCoordinatesReference(sensorPose3D);

	const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose());
	insertPointCloud(sensorPt, pts, o.maxRange);

	MRPT_END
}

void COccupancyGridMap3D::insertRay(
	const mrpt::math::TPoint3D& sensor, const mrpt::math::TPoint3D& end,
	bool endIsOccupied)
{
	MRPT_START

	// m_likelihoodCacheOutDated = true;

	// the occupied and free probabilities:
	const float maxCertainty = insertionOptions.maxOccupancyUpdateCertainty;
	float maxFreeCertainty = insertionOptions.maxFreenessUpdateCertainty;
	if (maxFreeCertainty == .0f) maxFreeCertainty = maxCertainty;

	const voxelType logodd_observation_free =
		std::max<voxelType>(1, p2l(maxFreeCertainty));
	const voxelType logodd_observation_occupied =
		3 * std::max<voxelType>(1, p2l(maxCertainty));

	// saturation limits:
	const voxelType logodd_thres_occupied =
		CLogOddsGridMap3D<voxelType>::CELLTYPE_MIN +
		logodd_observation_occupied;
	const voxelType logodd_thres_free =
		CLogOddsGridMap3D<voxelType>::CELLTYPE_MAX - logodd_observation_free;

	// Start: (in cell index units)
	int cx = m_grid.x2idx(sensor.x);
	int cy = m_grid.y2idx(sensor.y);
	int cz = m_grid.z2idx(sensor.z);

	// End: (in cell index units)
	int trg_cx = m_grid.x2idx(end.x);
	int trg_cy = m_grid.y2idx(end.y);
	int trg_cz = m_grid.z2idx(end.z);

	// Skip if totally out of bounds:
	if (m_grid.isOutOfBounds(cx, cy, cz)) return;

	// The occupied cell at the end:
	updateCell_fast_occupied(
		trg_cx, trg_cy, trg_cz, logodd_observation_occupied,
		logodd_thres_occupied);

	if (!insertionOptions.raytraceEmptyCells) return;  // done!

	// Use "fractional integers" to approximate float operations
	//  during the ray tracing:
	const int Acx = trg_cx - cx;
	const int Acy = trg_cy - cy;
	const int Acz = trg_cz - cz;

	const int Acx_ = std::abs(Acx);
	const int Acy_ = std::abs(Acy);
	const int Acz_ = std::abs(Acz);

	const int nStepsRay = mrpt::max3(Acx_, Acy_, Acz_);
	if (!nStepsRay) return;	 // May be...

	const float N_1 = 1.0f / nStepsRay;

	// Increments at each raytracing step:
	const int frAcx = (Acx < 0 ? -1 : +1) * round((Acx_ << FRBITS) * N_1);
	const int frAcy = (Acy < 0 ? -1 : +1) * round((Acy_ << FRBITS) * N_1);
	const int frAcz = (Acz < 0 ? -1 : +1) * round((Acz_ << FRBITS) * N_1);

	// fractional integers for the running raytracing point:
	int frCX = cx << FRBITS;
	int frCY = cy << FRBITS;
	int frCZ = cz << FRBITS;

	for (int nStep = 0; nStep < nStepsRay; nStep++)
	{
		updateCell_fast_free(
			cx, cy, cz, logodd_observation_free, logodd_thres_free);

		frCX += frAcx;
		frCY += frAcy;
		frCZ += frAcz;

		cx = frCX >> FRBITS;
		cy = frCY >> FRBITS;
		cz = frCZ >> FRBITS;

		// Already out of bounds?
		if (m_grid.isOutOfBounds(cx, cy, cz)) break;
	}

	MRPT_END
}

void COccupancyGridMap3D::internal_insertObservationPointCloud(
	const mrpt::obs::CObservationPointCloud& o,
	const mrpt::poses::CPose3D& robotPose)
{
	MRPT_START

	if (!o.pointcloud) return;

	const auto sensorPose3D = robotPose + o.sensorPose;

	const auto sensorPt = mrpt::math::TPoint3D(sensorPose3D.asTPose());
	insertPointCloud(
		sensorPt, *o.pointcloud, insertionOptions.maxDistanceInsertion,
		sensorPose3D);

	MRPT_END
}

void COccupancyGridMap3D::OnPostSuccesfulInsertObs(
	const mrpt::obs::CObservation&)
{
	m_is_empty = false;
}

void COccupancyGridMap3D::TLikelihoodOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
	likelihoodMethod =
		iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);

	MRPT_LOAD_CONFIG_VAR(LF_stdHit, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LF_zHit, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LF_zRandom, float, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LF_maxRange, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LF_decimation, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LF_maxCorrsDistance, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(LF_useSquareDist, int, iniFile, section);

	MRPT_LOAD_CONFIG_VAR(rayTracing_stdHit, int, iniFile, section);
	MRPT_LOAD_CONFIG_VAR(rayTracing_decimation, int, iniFile, section);
}

void COccupancyGridMap3D::TLikelihoodOptions::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_stdHit, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_zHit, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_zRandom, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_maxRange, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_decimation, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_maxCorrsDistance, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(LF_useSquareDist, "");

	MRPT_SAVE_CONFIG_VAR_COMMENT(rayTracing_stdHit, "");
	MRPT_SAVE_CONFIG_VAR_COMMENT(rayTracing_decimation, "");
}
