/* +------------------------------------------------------------------------+
   |                     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 "detectors-precomp.h"	// Precompiled headers
//
#include <mrpt/detectors/CDetectorDoorCrossing.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/poses/CPosePDF.h>

using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::detectors;
using namespace mrpt::poses;

/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CDetectorDoorCrossing::CDetectorDoorCrossing()
	: COutputLogger("CDetectorDoorCrossing"),
	  options(),
	  lastObs(),
	  entropy(),
	  lastEntropy()

{
	clear();
}

/*---------------------------------------------------------------
						clear
  ---------------------------------------------------------------*/
void CDetectorDoorCrossing::clear()
{
	lastObs.clear();
	lastEntropyValid = false;
}

/*---------------------------------------------------------------
						process
  ---------------------------------------------------------------*/
void CDetectorDoorCrossing::process(
	CActionRobotMovement2D& in_poseChange, CSensoryFrame& in_sf,
	TDoorCrossingOutParams& out_estimation)
{
	// Variables for generic use:
	size_t i;

	out_estimation.cumulativeTurning = 0;

	MRPT_START

	// 1) Add new pair to the list:
	// -----------------------------------------
	lastObs.insert(in_poseChange);
	lastObs.insert(in_sf);

	// 2) Remove oldest pair:
	// -----------------------------------------
	ASSERT_(options.windowSize > 1);
	ASSERT_((lastObs.size() % 2) == 0);	 // Assure even size

	while (lastObs.size() > options.windowSize * 2)
	{
		lastObs.remove(0);
		lastObs.remove(0);
	}

	if (lastObs.size() < options.windowSize * 2)
	{
		// Not enough old data yet:
		out_estimation.enoughInformation = false;
		return;
	}

	// 3) Build an occupancy grid map with observations
	// -------------------------------------------------
	CPose2D p, pos;

	TSetOfMetricMapInitializers mapInitializer;

	{
		CSimplePointsMap::TMapDefinition def;
		mapInitializer.push_back(def);
	}
	{
		COccupancyGridMap2D::TMapDefinition def;
		def.resolution = options.gridResolution;
		mapInitializer.push_back(def);
	}

	CMultiMetricMap auxMap(mapInitializer);

	for (i = 0; i < options.windowSize; i++)
	{
		CActionCollection::Ptr acts = lastObs.getAsAction(i * 2 + 0);
		CAction::Ptr act = acts->get(0);

		ASSERT_(act->GetRuntimeClass()->derivedFrom(
			CLASS_ID(CActionRobotMovement2D)));
		CActionRobotMovement2D::Ptr action =
			std::dynamic_pointer_cast<CActionRobotMovement2D>(act);

		action->poseChange->getMean(pos);

		out_estimation.cumulativeTurning += fabs(pos.phi());

		// Get the cumulative pose for the current observation:
		p = p + pos;

		// Add SF to the grid map:
		CSensoryFrame::Ptr sf = lastObs.getAsObservations(i * 2 + 1);
		CPose3D pose3D(p);
		sf->insertObservationsInto(auxMap, pose3D);
	}

	// 4) Compute the information differece between this
	//      "map patch" and the previous one:
	// -------------------------------------------------------
	auxMap.mapByClass<COccupancyGridMap2D>()->computeEntropy(entropy);

	if (!lastEntropyValid) { out_estimation.enoughInformation = false; }
	else
	{
		// 5) Fill output data
		// ---------------------------------
		out_estimation.enoughInformation = true;

		out_estimation.informationGain = entropy.I - lastEntropy.I;
		out_estimation.pointsMap = *auxMap.mapByClass<CSimplePointsMap>();
	}

	// For next iterations:
	lastEntropy = entropy;
	lastEntropyValid = true;

	MRPT_END
}
