/* +------------------------------------------------------------------------+
   |                     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 "vision-precomp.h"	 // Precompiled headers
//
#include <mrpt/math/ops_matrices.h>
#include <mrpt/vision/CFeatureExtraction.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::img;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace std;

void CFeatureExtraction::internal_computeSpinImageDescriptors(
	const CImage& in_img, CFeatureList& in_features)
{
	MRPT_START
	mrpt::system::CTimeLoggerEntry tle(
		profiler, "internal_computeSpinImageDescriptors");

	ASSERT_(options.SpinImagesOptions.radius > 1);

	// This is a C++ implementation of the descriptor "Intensity-domain spin
	// images" as described
	//  in "A sparse texture representation using affine-invariant regions", S
	//  Lazebnik, C Schmid, J Ponce,
	//  2003 IEEE Computer Society Conference on Computer Vision.

	const unsigned int HIST_N_INT =
		options.SpinImagesOptions.hist_size_intensity;
	const unsigned int HIST_N_DIS =
		options.SpinImagesOptions.hist_size_distance;
	const unsigned int R = options.SpinImagesOptions.radius;
	const int img_w = static_cast<int>(in_img.getWidth());
	const int img_h = static_cast<int>(in_img.getHeight());
	const bool img_color = in_img.isColor();

	// constant for passing intensity [0,255] to histogram index
	// [0,HIST_N_INT-1]:
	const float k_int2idx = (HIST_N_INT - 1) / 255.0f;
	const float k_idx2int = 1.0f / k_int2idx;

	// constant for passing distances in pixels [0,R] to histogram index
	// [0,HIST_N_DIS-1]:
	const float k_dis2idx = (HIST_N_DIS - 1) / d2f(R);
	const float k_idx2dis = 1.0f / k_dis2idx;

	// The Gaussian kernel used below is approximated up to a given distance
	//  in pixels, given by 2 times the appropriate std. deviations:
	const int STD_TIMES = 2;
	const int kernel_size_dist = static_cast<int>(
		ceil(k_dis2idx * STD_TIMES * options.SpinImagesOptions.std_dist));

	const float _2var_int =
		-1.0f / (2 * square(options.SpinImagesOptions.std_intensity));
	const float _2var_dist =
		-1.0f / (2 * square(options.SpinImagesOptions.std_dist));

	// Create the 2D histogram:
	CMatrixDouble hist2d(HIST_N_INT, HIST_N_DIS);

	// Compute intensity-domain spin images
	for (auto& in_feature : in_features)
	{
		// Overwrite scale with the descriptor scale:
		in_feature.keypoint.octave = options.SpinImagesOptions.radius;

		// Reset histogram to zeros:
		hist2d.setZero();

		// Define the ROI around the interest point which counts for the
		// histogram:
		int px0 = round(in_feature.keypoint.pt.x - R);
		int px1 = round(in_feature.keypoint.pt.x + R);
		int py0 = round(in_feature.keypoint.pt.y - R);
		int py1 = round(in_feature.keypoint.pt.y + R);

		// Clip at img borders:
		px0 = max(0, px0);
		px1 = min(img_w - 1, px1);
		py0 = max(0, py0);
		py1 = min(img_h - 1, py1);

		for (int px = px0; px <= px1; px++)
		{
			for (int py = py0; py <= py1; py++)
			{
				uint8_t pix_val;
				// get the pixel color [0,255]
				if (!img_color) pix_val = in_img.at<uint8_t>(px, py);
				else
				{
					auto aux_pix_ptr = in_img.ptr<uint8_t>(px, py);
					pix_val =
						(aux_pix_ptr[0] + aux_pix_ptr[1] + aux_pix_ptr[2]) / 3;
				}

				const float pix_dist = hypot(
					in_feature.keypoint.pt.x - px,
					in_feature.keypoint.pt.y - py);
				const int center_bin_dist = k_dis2idx * pix_dist;

				// A factor to correct the histogram due to the existence of
				// more pixels at larger radius:
				//  Obtained as Area = PI ( R1^2 - R2^2 ) for R1,R2 being
				//  pix_dist +- Delta/2
				// const double density_circular_area = 1.0/ (M_2PI *
				// (center_bin_dist+0.5) * square(k_idx2dis) );

				// Apply a "soft-histogram", so each pixel counts into several
				// bins,
				//  weighted by a exponential function to model a Gaussian
				//  kernel
				const int bin_int_low =
					max(0,
						static_cast<int>(ceil(
							k_int2idx *
							(pix_val -
							 STD_TIMES *
								 options.SpinImagesOptions.std_intensity))));
				const int bin_int_hi =
					min(static_cast<int>(HIST_N_INT - 1),
						static_cast<int>(ceil(
							k_int2idx *
							(pix_val +
							 STD_TIMES *
								 options.SpinImagesOptions.std_intensity))));

				// cout << "d: " << pix_dist << "v: " << (int)pix_val << "\t";

				if (center_bin_dist <
					static_cast<int>(HIST_N_DIS))  // this accounts for the
				// "square" or "circle"
				// shapes of the area to
				// account for
				{
					const int bin_dist_low =
						max(0, center_bin_dist - kernel_size_dist);
					const int bin_dist_hi =
						min(static_cast<int>(HIST_N_DIS - 1),
							center_bin_dist + kernel_size_dist);

					int bin_dist, bin_int;
					float pix_dist_cur_dist =
						pix_dist - bin_dist_low * k_idx2dis;

					for (bin_dist = bin_dist_low; bin_dist <= bin_dist_hi;
						 bin_dist++, pix_dist_cur_dist -= k_idx2dis)
					{
						float pix_val_cur_val =
							pix_val - (bin_int_low * k_idx2int);

						for (bin_int = bin_int_low; bin_int <= bin_int_hi;
							 bin_int++, pix_val_cur_val -= k_idx2int)
						{
							// Gaussian kernel:
							double v = _2var_dist * square(pix_dist_cur_dist) +
								_2var_int * square(pix_val_cur_val);

							hist2d(bin_int, bin_dist) += exp(v);
						}
					}
					// hist2d(bin_int,bin_dist) *= ;
				}  // in range

			}  // end py
		}  // end px

		// Normalize [0,1]
		mrpt::math::normalize(hist2d, 0, 1);

		// Save the histogram as a vector:
		unsigned idx = 0;
		in_feature.descriptors.SpinImg.emplace();
		std::vector<float>& ptr_trg = *in_feature.descriptors.SpinImg;
		ptr_trg.resize(HIST_N_INT * HIST_N_DIS);

		for (unsigned i = 0; i < HIST_N_DIS; i++)
			for (unsigned j = 0; j < HIST_N_INT; j++)
				ptr_trg[idx++] = hist2d(j, i);

		in_feature.descriptors.SpinImg_range_rows = HIST_N_DIS;

	}  // end for each feature

	MRPT_END
}
