/* +---------------------------------------------------------------------------+
   |                     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 details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
   */
#include "CLandmarksConfig.h"

#include "TypeOfConfig.h"
#include "ui_CLandmarksConfig.h"

using namespace mrpt;
using namespace maps;
using namespace vision;

CLandmarksConfig::CLandmarksConfig()
	: CBaseConfig(), m_ui(std::make_unique<Ui::CLandmarksConfig>())
{
	m_ui->setupUi(this);
	m_ui->TKeyPointMethod->addItem(
		"featNotDefined", TKeyPointMethod::featNotDefined);
	m_ui->TKeyPointMethod->addItem("featKLT", TKeyPointMethod::featKLT);
	m_ui->TKeyPointMethod->addItem("featHarris", TKeyPointMethod::featHarris);
	m_ui->TKeyPointMethod->addItem("featSIFT", TKeyPointMethod::featSIFT);
	m_ui->TKeyPointMethod->addItem("featSURF", TKeyPointMethod::featSURF);
	m_ui->TKeyPointMethod->addItem("featBeacon", TKeyPointMethod::featBeacon);
	m_ui->TKeyPointMethod->addItem("featFAST", TKeyPointMethod::featFAST);

	m_ui->implementation->addItem("OpenCV", CFeatureExtraction::OpenCV);

	setInsertOpt();
	setLikelihoodOpt();
}

const QString CLandmarksConfig::getName()
{
	return QString::fromStdString(typeToName(TypeOfConfig::Landmarks));
}

void CLandmarksConfig::updateConfiguration(
	mrpt::maps::TMetricMapInitializer* options)
{
	auto* mapDefination = dynamic_cast<CLandmarksMap::TMapDefinition*>(options);
	ASSERT_(mapDefination);

	mapDefination->genericMapParams.enableSaveAs3DObject =
		m_ui->enableSaveAs3DObject;
	mapDefination->genericMapParams.enableObservationLikelihood =
		m_ui->enableObservationLikelihood;
	mapDefination->genericMapParams.enableObservationInsertion =
		m_ui->enableObservationInsertion;

	mapDefination->insertionOpts.insert_SIFTs_from_monocular_images =
		m_ui->insert_SIFTs_from_monocular_images->isChecked();
	mapDefination->insertionOpts.insert_SIFTs_from_stereo_images =
		m_ui->insert_SIFTs_from_stereo_images->isChecked();
	mapDefination->insertionOpts.insert_Landmarks_from_range_scans =
		m_ui->insert_Landmarks_from_range_scans->isChecked();

	mapDefination->insertionOpts.SiftCorrRatioThreshold =
		m_ui->SiftCorrRatioThreshold->value();
	mapDefination->insertionOpts.SiftLikelihoodThreshold =
		m_ui->SiftLikelihoodThreshold->value();
	mapDefination->insertionOpts.SiftEDDThreshold =
		m_ui->SiftEDDThreshold->value();
	mapDefination->insertionOpts.SIFTMatching3DMethod =
		m_ui->SIFTMatching3DMethod->value();

	mapDefination->insertionOpts.SIFTsLoadDistanceOfTheMean =
		m_ui->SIFTsLoadDistanceOfTheMean->value();
	mapDefination->insertionOpts.SIFTsLoadEllipsoidWidth =
		m_ui->SIFTsLoadEllipsoidWidth->value();
	mapDefination->insertionOpts.SIFTs_stdXY = m_ui->SIFTs_stdXY->value();
	mapDefination->insertionOpts.SIFTs_stdDisparity =
		m_ui->SIFTs_stdDisparity->value();
	mapDefination->insertionOpts.SIFTs_numberOfKLTKeypoints =
		m_ui->SIFTs_numberOfKLTKeypoints->value();
	mapDefination->insertionOpts.SIFTs_stereo_maxDepth =
		m_ui->SIFTs_stereo_maxDepth->value();
	mapDefination->insertionOpts.SIFTs_epipolar_TH =
		m_ui->SIFTs_epipolar_TH->value();
	mapDefination->insertionOpts.PLOT_IMAGES = m_ui->PLOT_IMAGES->isChecked();

	mapDefination->likelihoodOpts.rangeScan2D_decimation =
		m_ui->rangeScan2D_decimation->value();
	mapDefination->likelihoodOpts.SIFTs_sigma_euclidean_dist =
		m_ui->SIFTs_sigma_euclidean_dist->value();
	mapDefination->likelihoodOpts.SIFTs_sigma_descriptor_dist =
		m_ui->SIFTs_sigma_descriptor_dist->value();
	mapDefination->likelihoodOpts.SIFTs_mahaDist_std =
		m_ui->SIFTs_mahaDist_std->value();
	mapDefination->likelihoodOpts.SIFTnullCorrespondenceDistance =
		m_ui->SIFTnullCorrespondenceDistance->value();
	mapDefination->likelihoodOpts.SIFTs_decimation =
		m_ui->SIFTs_decimation->value();  // int

	mapDefination->likelihoodOpts.beaconRangesStd =
		m_ui->beaconRangesStd->value();
	mapDefination->likelihoodOpts.beaconRangesUseObservationStd =
		m_ui->beaconRangesUseObservationStd->isChecked();
	mapDefination->likelihoodOpts.extRobotPoseStd =
		m_ui->extRobotPoseStd->value();
	mapDefination->likelihoodOpts.GPS_sigma = m_ui->GPS_sigma->value();

	mapDefination->likelihoodOpts.GPSOrigin.longitude =
		m_ui->longitude->value();
	mapDefination->likelihoodOpts.GPSOrigin.latitude = m_ui->latitude->value();
	mapDefination->likelihoodOpts.GPSOrigin.altitude = m_ui->altitude->value();
	mapDefination->likelihoodOpts.GPSOrigin.ang = m_ui->ang->value();
	mapDefination->likelihoodOpts.GPSOrigin.x_shift = m_ui->x_shift->value();
	mapDefination->likelihoodOpts.GPSOrigin.y_shift = m_ui->y_shift->value();
	mapDefination->likelihoodOpts.GPSOrigin.min_sat = m_ui->min_sat->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.featsType =
		static_cast<TKeyPointMethod>(
			m_ui->TKeyPointMethod->currentData().toInt());
	mapDefination->likelihoodOpts.SIFT_feat_options.patchSize =
		m_ui->patchSize->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.useMask =
		m_ui->useMask->isChecked();
	mapDefination->likelihoodOpts.SIFT_feat_options.addNewFeatures =
		m_ui->addNewFeatures->isChecked();
	mapDefination->likelihoodOpts.SIFT_feat_options.FIND_SUBPIXEL =
		m_ui->FIND_SUBPIXEL->isChecked();

	mapDefination->likelihoodOpts.SIFT_feat_options.KLTOptions.radius =
		m_ui->radiusKLTOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.KLTOptions.threshold =
		m_ui->thresholdKLTOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.KLTOptions.min_distance =
		m_ui->min_distanceKLTOptions->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.harrisOptions.threshold =
		m_ui->thresholdHarris->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.harrisOptions.k =
		m_ui->k->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.harrisOptions.sigma =
		m_ui->sigma->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.harrisOptions.radius =
		m_ui->radiusHarris->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.harrisOptions.min_distance =
		m_ui->min_distanceHarris->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.FASTOptions.threshold =
		m_ui->thresholdFASTOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.FASTOptions.min_distance =
		m_ui->min_distanceFASTOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.FASTOptions
		.nonmax_suppression = m_ui->nonmax_suppression->isChecked();
	mapDefination->likelihoodOpts.SIFT_feat_options.FASTOptions
		.use_KLT_response = m_ui->use_KLT_response->isChecked();

	mapDefination->likelihoodOpts.SIFT_feat_options.ORBOptions.n_levels =
		m_ui->n_levels->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.ORBOptions.min_distance =
		m_ui->min_distanceORBOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.ORBOptions.scale_factor =
		m_ui->scale_factor->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.ORBOptions.extract_patch =
		m_ui->extract_patch->isChecked();

	mapDefination->likelihoodOpts.SIFT_feat_options.SIFTOptions.threshold =
		m_ui->thresholdSIFTOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SIFTOptions.edgeThreshold =
		m_ui->edgeThreshold->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.SURFOptions
		.rotation_invariant = m_ui->rotation_invariant->isChecked();
	mapDefination->likelihoodOpts.SIFT_feat_options.SURFOptions
		.hessianThreshold = m_ui->hessianThreshold->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SURFOptions.nOctaves =
		m_ui->nOctaves->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SURFOptions
		.nLayersPerOctave = m_ui->nLayersPerOctave->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.SpinImagesOptions
		.hist_size_intensity = m_ui->hist_size_intensity->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SpinImagesOptions
		.hist_size_distance = m_ui->hist_size_distance->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SpinImagesOptions.std_dist =
		m_ui->std_dist->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SpinImagesOptions
		.std_intensity = m_ui->std_intensity->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.SpinImagesOptions.radius =
		m_ui->radiusSpinImagesOptions->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.PolarImagesOptions
		.bins_angle = m_ui->bins_angle->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.PolarImagesOptions
		.bins_distance = m_ui->bins_distance->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.PolarImagesOptions.radius =
		m_ui->radiusPolarImagesOptions->value();

	mapDefination->likelihoodOpts.SIFT_feat_options.LogPolarImagesOptions
		.radius = m_ui->radiusLogPolarImagesOptions->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.LogPolarImagesOptions
		.num_angles = m_ui->num_angles->value();
	mapDefination->likelihoodOpts.SIFT_feat_options.LogPolarImagesOptions
		.rho_scale = m_ui->rho_scale->value();
}

TypeOfConfig CLandmarksConfig::type() const { return TypeOfConfig::Landmarks; }
void CLandmarksConfig::setInsertOpt(
	const CLandmarksMap::TInsertionOptions& insertOpt)
{
	m_ui->insert_SIFTs_from_monocular_images->setChecked(
		insertOpt.insert_SIFTs_from_monocular_images);
	m_ui->insert_SIFTs_from_stereo_images->setChecked(
		insertOpt.insert_SIFTs_from_stereo_images);
	m_ui->insert_Landmarks_from_range_scans->setChecked(
		insertOpt.insert_Landmarks_from_range_scans);

	m_ui->SiftCorrRatioThreshold->setValue(
		insertOpt.insert_Landmarks_from_range_scans);
	m_ui->SiftLikelihoodThreshold->setValue(
		insertOpt.insert_Landmarks_from_range_scans);
	m_ui->SiftEDDThreshold->setValue(
		insertOpt.insert_Landmarks_from_range_scans);
	m_ui->SIFTMatching3DMethod->setValue(insertOpt.SIFTMatching3DMethod);

	m_ui->SIFTsLoadDistanceOfTheMean->setValue(
		insertOpt.SIFTsLoadDistanceOfTheMean);
	m_ui->SIFTsLoadEllipsoidWidth->setValue(insertOpt.SIFTsLoadEllipsoidWidth);
	m_ui->SIFTs_stdXY->setValue(insertOpt.SIFTs_stdXY);
	m_ui->SIFTs_stdDisparity->setValue(insertOpt.SIFTs_stdDisparity);
	m_ui->SIFTs_numberOfKLTKeypoints->setValue(
		insertOpt.SIFTs_numberOfKLTKeypoints);
	m_ui->SIFTs_stereo_maxDepth->setValue(insertOpt.SIFTs_stereo_maxDepth);
	m_ui->SIFTs_epipolar_TH->setValue(insertOpt.SIFTs_epipolar_TH);
	m_ui->PLOT_IMAGES->setChecked(insertOpt.PLOT_IMAGES);
}

void CLandmarksConfig::setLikelihoodOpt(
	const CLandmarksMap::TLikelihoodOptions& likelihoodOpt)
{
	m_ui->rangeScan2D_decimation->setValue(
		likelihoodOpt.rangeScan2D_decimation);
	m_ui->SIFTs_sigma_euclidean_dist->setValue(
		likelihoodOpt.SIFTs_sigma_euclidean_dist);
	m_ui->SIFTs_sigma_descriptor_dist->setValue(
		likelihoodOpt.SIFTs_sigma_descriptor_dist);
	m_ui->SIFTs_mahaDist_std->setValue(likelihoodOpt.SIFTs_mahaDist_std);
	m_ui->SIFTnullCorrespondenceDistance->setValue(
		likelihoodOpt.SIFTnullCorrespondenceDistance);
	m_ui->SIFTs_decimation->setValue(likelihoodOpt.SIFTs_decimation);

	m_ui->beaconRangesStd->setValue(likelihoodOpt.beaconRangesStd);
	m_ui->beaconRangesUseObservationStd->setChecked(
		likelihoodOpt.beaconRangesUseObservationStd);
	m_ui->extRobotPoseStd->setValue(likelihoodOpt.extRobotPoseStd);
	m_ui->GPS_sigma->setValue(likelihoodOpt.GPS_sigma);

	m_ui->longitude->setValue(likelihoodOpt.GPSOrigin.longitude);
	m_ui->latitude->setValue(likelihoodOpt.GPSOrigin.latitude);
	m_ui->altitude->setValue(likelihoodOpt.GPSOrigin.altitude);
	m_ui->ang->setValue(likelihoodOpt.GPSOrigin.ang);
	m_ui->x_shift->setValue(likelihoodOpt.GPSOrigin.x_shift);
	m_ui->y_shift->setValue(likelihoodOpt.GPSOrigin.y_shift);
	m_ui->min_sat->setValue(likelihoodOpt.GPSOrigin.min_sat);

	m_ui->TKeyPointMethod->setCurrentIndex(
		likelihoodOpt.SIFT_feat_options.featsType + 1);

	m_ui->patchSize->setValue(likelihoodOpt.SIFT_feat_options.patchSize);

	m_ui->useMask->setChecked(likelihoodOpt.SIFT_feat_options.useMask);
	m_ui->addNewFeatures->setChecked(
		likelihoodOpt.SIFT_feat_options.addNewFeatures);
	m_ui->FIND_SUBPIXEL->setChecked(
		likelihoodOpt.SIFT_feat_options.FIND_SUBPIXEL);

	m_ui->radiusKLTOptions->setValue(
		likelihoodOpt.SIFT_feat_options.KLTOptions.radius);
	m_ui->thresholdKLTOptions->setValue(
		likelihoodOpt.SIFT_feat_options.KLTOptions.threshold);
	m_ui->min_distanceKLTOptions->setValue(
		likelihoodOpt.SIFT_feat_options.KLTOptions.min_distance);

	m_ui->thresholdHarris->setValue(
		likelihoodOpt.SIFT_feat_options.harrisOptions.threshold);
	m_ui->k->setValue(likelihoodOpt.SIFT_feat_options.harrisOptions.k);
	m_ui->sigma->setValue(likelihoodOpt.SIFT_feat_options.harrisOptions.sigma);
	m_ui->radiusHarris->setValue(
		likelihoodOpt.SIFT_feat_options.harrisOptions.radius);
	m_ui->min_distanceHarris->setValue(
		likelihoodOpt.SIFT_feat_options.harrisOptions.min_distance);

	m_ui->thresholdFASTOptions->setValue(
		likelihoodOpt.SIFT_feat_options.FASTOptions.threshold);
	m_ui->min_distanceFASTOptions->setValue(
		likelihoodOpt.SIFT_feat_options.FASTOptions.min_distance);
	m_ui->nonmax_suppression->setChecked(
		likelihoodOpt.SIFT_feat_options.FASTOptions.nonmax_suppression);
	m_ui->use_KLT_response->setChecked(
		likelihoodOpt.SIFT_feat_options.FASTOptions.use_KLT_response);

	m_ui->n_levels->setValue(
		likelihoodOpt.SIFT_feat_options.ORBOptions.n_levels);
	m_ui->min_distanceORBOptions->setValue(
		likelihoodOpt.SIFT_feat_options.ORBOptions.min_distance);
	m_ui->scale_factor->setValue(
		likelihoodOpt.SIFT_feat_options.ORBOptions.scale_factor);
	m_ui->extract_patch->setChecked(
		likelihoodOpt.SIFT_feat_options.ORBOptions.extract_patch);

	m_ui->thresholdSIFTOptions->setValue(
		likelihoodOpt.SIFT_feat_options.SIFTOptions.threshold);
	m_ui->edgeThreshold->setValue(
		likelihoodOpt.SIFT_feat_options.SIFTOptions.edgeThreshold);

	m_ui->rotation_invariant->setChecked(
		likelihoodOpt.SIFT_feat_options.SURFOptions.rotation_invariant);
	m_ui->hessianThreshold->setValue(
		likelihoodOpt.SIFT_feat_options.SURFOptions.hessianThreshold);
	m_ui->nOctaves->setValue(
		likelihoodOpt.SIFT_feat_options.SURFOptions.nOctaves);
	m_ui->nLayersPerOctave->setValue(
		likelihoodOpt.SIFT_feat_options.SURFOptions.nLayersPerOctave);

	m_ui->hist_size_intensity->setValue(
		likelihoodOpt.SIFT_feat_options.SpinImagesOptions.hist_size_intensity);
	m_ui->hist_size_distance->setValue(
		likelihoodOpt.SIFT_feat_options.SpinImagesOptions.hist_size_distance);
	m_ui->std_dist->setValue(
		likelihoodOpt.SIFT_feat_options.SpinImagesOptions.std_dist);
	m_ui->std_intensity->setValue(
		likelihoodOpt.SIFT_feat_options.SpinImagesOptions.std_intensity);
	m_ui->radiusSpinImagesOptions->setValue(
		likelihoodOpt.SIFT_feat_options.SpinImagesOptions.radius);

	m_ui->bins_angle->setValue(
		likelihoodOpt.SIFT_feat_options.PolarImagesOptions.bins_angle);
	m_ui->bins_distance->setValue(
		likelihoodOpt.SIFT_feat_options.PolarImagesOptions.bins_distance);
	m_ui->radiusPolarImagesOptions->setValue(
		likelihoodOpt.SIFT_feat_options.PolarImagesOptions.radius);

	m_ui->radiusLogPolarImagesOptions->setValue(
		likelihoodOpt.SIFT_feat_options.LogPolarImagesOptions.radius);
	m_ui->num_angles->setValue(
		likelihoodOpt.SIFT_feat_options.LogPolarImagesOptions.num_angles);
	m_ui->rho_scale->setValue(
		likelihoodOpt.SIFT_feat_options.LogPolarImagesOptions.rho_scale);
}
