/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://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::utils;
using namespace mrpt::poses;



/*---------------------------------------------------------------
						Constructor
  ---------------------------------------------------------------*/
CDetectorDoorCrossing::CDetectorDoorCrossing()  :
	options(),
	lastObs(),
	entropy(),
	lastEntropy(),
	lastEntropyValid(false)
{
	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.addAction( in_poseChange );
	lastObs.addObservations( 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 enought old data yet:
		out_estimation.enoughtInformation = 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++)
	{
		CActionCollectionPtr	acts = lastObs.getAsAction( i*2+0 );
		CActionPtr				act = acts->get(0);

		ASSERT_( act->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
		CActionRobotMovement2DPtr action = CActionRobotMovement2DPtr( 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:
		CSensoryFramePtr	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.m_gridMaps[0]->computeEntropy( entropy );

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


		out_estimation.informationGain = entropy.I - lastEntropy.I;
		out_estimation.pointsMap.copyFrom( *auxMap.m_pointsMaps[0] );
	}


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

	MRPT_END

}

