/* +---------------------------------------------------------------------------+
   |                     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 "vision-precomp.h"   // Precompiled headers

#include <mrpt/vision/CFeatureExtraction.h>

// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h> 

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::vision;
using namespace mrpt::system;
using namespace std;

/************************************************************************************************
*								extractFeaturesORB												*
************************************************************************************************/
void  CFeatureExtraction::extractFeaturesORB(
	const mrpt::utils::CImage	& inImg,
	CFeatureList			    & feats,
	const unsigned int			init_ID,
	const unsigned int			nDesiredFeatures,
	const TImageROI			    & ROI )  const
{
	MRPT_UNUSED_PARAM(ROI);
	MRPT_START

#if MRPT_HAS_OPENCV
#	if MRPT_OPENCV_VERSION_NUM < 0x240
		THROW_EXCEPTION("This function requires OpenCV > 2.4.0")
#	else

	using namespace cv;

	vector<KeyPoint> cv_feats; // OpenCV keypoint output vector
	Mat              cv_descs; // OpenCV descriptor output

	const bool use_precomputed_feats = feats.size() > 0;

	if( use_precomputed_feats )
	{
		cv_feats.resize( feats.size() );
		for( size_t k = 0; k < cv_feats.size(); ++k )
		{
			cv_feats[k].pt.x = feats[k]->x;
			cv_feats[k].pt.y = feats[k]->y;
		}
	}
	
	// Make sure we operate on a gray-scale version of the image:
	const CImage inImg_gray( inImg, FAST_REF_OR_CONVERT_TO_GRAY );
	const Mat cvImg = cv::cvarrToMat( inImg_gray.getAs<IplImage>() );

	// The detector and descriptor
#	if MRPT_OPENCV_VERSION_NUM < 0x300
	Ptr<Feature2D> orb = Algorithm::create<Feature2D>("Feature2D.ORB");
	orb->operator()( cvImg, Mat(), cv_feats, cv_descs, use_precomputed_feats );
#else
	const size_t n_feats_2_extract = nDesiredFeatures == 0 ? 1000 : 3*nDesiredFeatures;
	Ptr<cv::ORB> orb = cv::ORB::create( n_feats_2_extract, options.ORBOptions.scale_factor, options.ORBOptions.n_levels );
	orb->detectAndCompute(cvImg, Mat(), cv_feats, cv_descs, use_precomputed_feats );
#endif
	
	const size_t n_feats = cv_feats.size();

	// if we had input features, just convert cv_feats to CFeatures and return
	const unsigned int patch_size_2	= options.patchSize/2;
	unsigned int f_id				= init_ID;
	if( use_precomputed_feats )
	{ 
		for( size_t k = 0; k < n_feats; ++k )
		{
			feats[k]->descriptors.ORB.resize( cv_descs.cols );
			for( int m = 0; m < cv_descs.cols; ++m )
				feats[k]->descriptors.ORB[m] = cv_descs.at<uchar>(k,m);		// to do: memcopy

			/*
			feats[k].response	= cv_feats[k].response;
			feats[k].scale		= cv_feats[k].size;
			feats[k].angle		= cv_feats[k].orientation;
			feats[k].ID			= f_id++;
			*/
			feats[k]->type		= featORB;

			if( options.ORBOptions.extract_patch && options.patchSize > 0 )
			{
				inImg.extract_patch(
				feats[k]->patch,
				round( feats[k]->x ) - patch_size_2,
				round( feats[k]->y ) - patch_size_2,
				options.patchSize,
				options.patchSize );
			}
		}
		return;
	} 

	//  1) Sort the fearues by "response": It's ~100 times faster to sort a list of
	//      indices "sorted_indices" than sorting directly the actual list of features "cv_feats"
	std::vector<size_t> sorted_indices(n_feats);
	for (size_t i=0;i<n_feats;i++)  sorted_indices[i]=i;
	std::sort( sorted_indices.begin(), sorted_indices.end(), KeypointResponseSorter< vector<KeyPoint> >(cv_feats) );

	//  2) Filter by "min-distance" (in options.ORBOptions.min_distance)
	//  3) Convert to MRPT CFeatureList format.
	// Steps 2 & 3 are done together in the while() below.
	// The "min-distance" filter is done by means of a 2D binary matrix where each cell is marked when one
	// feature falls within it. This is not exactly the same than a pure "min-distance" but is pretty close
	// and for large numbers of features is much faster than brute force search of kd-trees.
	// (An intermediate approach would be the creation of a mask image updated for each accepted feature, etc.)

	const bool do_filter_min_dist = options.ORBOptions.min_distance > 1;
	const unsigned int occupied_grid_cell_size = options.ORBOptions.min_distance/2.0;
	const float occupied_grid_cell_size_inv = 1.0f/occupied_grid_cell_size;

	unsigned int grid_lx = !do_filter_min_dist ? 1 : (unsigned int)(1 + inImg.getWidth() * occupied_grid_cell_size_inv);
	unsigned int grid_ly = !do_filter_min_dist ? 1 : (unsigned int)(1 + inImg.getHeight() * occupied_grid_cell_size_inv );

	mrpt::math::CMatrixBool occupied_sections(grid_lx,grid_ly);  // See the comments above for an explanation.
	occupied_sections.fillAll(false);

	const size_t n_max_feats = nDesiredFeatures > 0 ? std::min(size_t(nDesiredFeatures),n_feats) : n_feats;

	if( !options.addNewFeatures )
        feats.clear();
	// feats.reserve( feats.size() + n_max_feats );

	const size_t imgH = inImg.getHeight();
	const size_t imgW = inImg.getWidth();
	size_t k = 0;
	size_t c_feats = 0;
	while( c_feats < n_max_feats && k < n_feats )
	{
		const size_t idx = sorted_indices[k++];
		const KeyPoint & kp = cv_feats[ idx ];
		if( options.ORBOptions.extract_patch && options.patchSize > 0 )
		{
			// check image boundaries for extracting the patch
			const int xBorderInf = (int)floor( kp.pt.x - patch_size_2 );
			const int xBorderSup = (int)floor( kp.pt.x + patch_size_2 );
			const int yBorderInf = (int)floor( kp.pt.y - patch_size_2 );
			const int yBorderSup = (int)floor( kp.pt.y + patch_size_2 );

			if (!( xBorderSup < (int)imgW && xBorderInf > 0 && yBorderSup < (int)imgH && yBorderInf > 0 ))
				continue; // nope, skip.
		}

		if (do_filter_min_dist)
		{
			// Check the min-distance:
			const size_t section_idx_x = size_t(kp.pt.x * occupied_grid_cell_size_inv);
			const size_t section_idx_y = size_t(kp.pt.y * occupied_grid_cell_size_inv);

			if (occupied_sections(section_idx_x,section_idx_y))
				continue; // Already occupied! skip.

			// Mark section as occupied
			occupied_sections.set_unsafe(section_idx_x,section_idx_y, true);
			if (section_idx_x>0)			occupied_sections.set_unsafe(section_idx_x-1,section_idx_y, true);
			if (section_idx_y>0)			occupied_sections.set_unsafe(section_idx_x,section_idx_y-1, true);
			if (section_idx_x<grid_lx-1)	occupied_sections.set_unsafe(section_idx_x+1,section_idx_y, true);
			if (section_idx_y<grid_ly-1)	occupied_sections.set_unsafe(section_idx_x,section_idx_y+1, true);
		}

		// All tests passed: add new feature:
		CFeaturePtr ft		= CFeature::Create();
		ft->type			= featORB;
		ft->ID				= f_id++;
		ft->x				= kp.pt.x;
		ft->y				= kp.pt.y;
		ft->response		= kp.response;
		ft->orientation		= kp.angle;
		ft->scale			= kp.octave;
		ft->patchSize		= 0;

		// descriptor
		ft->descriptors.ORB.resize( cv_descs.cols );
		for( int m = 0; m < cv_descs.cols; ++m )
			ft->descriptors.ORB[m] = cv_descs.at<uchar>(idx,m);

		if( options.ORBOptions.extract_patch && options.patchSize > 0 )
		{
			ft->patchSize	= options.patchSize;		// The size of the feature patch

			inImg.extract_patch(
				ft->patch,
				round( ft->x ) - patch_size_2,
				round( ft->y ) - patch_size_2,
				options.patchSize,
				options.patchSize );					// Image patch surronding the feature
		}
	
		feats.push_back( ft );
		c_feats++;
	}
#	endif
#endif
	MRPT_END
}

void CFeatureExtraction::internal_computeORBDescriptors( 
	const CImage	& in_img,
	CFeatureList	& in_features) const
{
#if MRPT_HAS_OPENCV
#	if MRPT_OPENCV_VERSION_NUM < 0x240
		THROW_EXCEPTION("This function requires OpenCV > 2.4.0")
#	else
	using namespace cv;

	const size_t n_feats = in_features.size(); 
	const CImage inImg_gray( in_img, FAST_REF_OR_CONVERT_TO_GRAY );

	// convert from CFeatureList to vector<KeyPoint>
	vector<KeyPoint> cv_feats( n_feats );
	for( size_t k = 0; k < n_feats; ++k )
	{
		KeyPoint & kp = cv_feats[k];
		kp.pt.x		= in_features[k]->x;
		kp.pt.y		= in_features[k]->y;
		kp.angle	= in_features[k]->orientation;
		kp.size		= in_features[k]->scale;
	} // end-for

	Mat cvImg(cv::cvarrToMat(inImg_gray.getAs<IplImage>()));
	Mat cv_descs;

#	if MRPT_OPENCV_VERSION_NUM < 0x300
	Ptr<Feature2D> orb = Algorithm::create<Feature2D>("Feature2D.ORB");
	orb->operator()( cvImg, Mat(), cv_feats, cv_descs, true /* use_precomputed_feats */ );
#else
	Ptr<cv::ORB> orb = cv::ORB::create( n_feats, options.ORBOptions.scale_factor, options.ORBOptions.n_levels );
	orb->detectAndCompute(cvImg, Mat(), cv_feats, cv_descs, true /* use_precomputed_feats */ );
#endif

	// add descriptor to CFeatureList
	for( size_t k = 0; k < n_feats; ++k )
	{
		in_features[k]->descriptors.ORB.resize( cv_descs.cols );
		for( int i = 0; i < cv_descs.cols; ++i )
			in_features[k]->descriptors.ORB[i] = cv_descs.at<uchar>(k,i);

	} // end-for
#	endif
#endif

} // end-internal_computeORBImageDescriptors


