// Author: Francesco Regazzoni - MOX, Politecnico di Milano
// Email:  francesco.regazzoni@polimi.it
// Date:   2020

#include <cmath>

#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include "model_RDQ18.hpp"

model_RDQ18::model_RDQ18(std::string parameters_file) : sarcomere("RDQ18") {
  // Read JSON options file
  boost::property_tree::ptree root;
  boost::property_tree::read_json(parameters_file, root);

  prm_n_RU = root.get_child("geometry")
                 .get_child("n_RU")
                 .get_value<unsigned int>(); // [-]
  prm_LA = root.get_child("geometry")
               .get_child("LA")
               .get_value<double>(); // [micro m]
  prm_LM = root.get_child("geometry")
               .get_child("LM")
               .get_value<double>(); // [micro m]
  prm_LB = root.get_child("geometry")
               .get_child("LB")
               .get_value<double>(); // [micro m]
  prm_Lsmooth = root.get_child("geometry")
                    .get_child("Lsmooth")
                    .get_value<double>(); // [micro m]
  prm_Q0 = root.get_child("RU_steady_state")
               .get_child("Q0")
               .get_value<double>(); // [-]
  prm_SLQ = root.get_child("RU_steady_state")
                .get_child("SLQ")
                .get_value<double>(); // [micro m]
  prm_alphaQ = root.get_child("RU_steady_state")
                   .get_child("alphaQ")
                   .get_value<double>(); // [1 / micro m]
  prm_mu = root.get_child("RU_steady_state")
               .get_child("mu")
               .get_value<double>(); // [-]
  prm_gamma = root.get_child("RU_steady_state")
                  .get_child("gamma")
                  .get_value<double>(); // [-]
  prm_Kon = root.get_child("RU_kinetics")
                .get_child("Kon")
                .get_value<double>(); // [micro M^-1 * s^-1]
  prm_Koff = root.get_child("RU_kinetics")
                 .get_child("Koff")
                 .get_value<double>(); // [s^-1]
  prm_Kbasic = root.get_child("RU_kinetics")
                   .get_child("Kbasic")
                   .get_value<double>(); // [s^-1]
  prm_TaMax = root.get_child("upscaling")
                  .get_child("TaMax")
                  .get_value<double>(); // [kPa]

  allocate_variables();

  initialize_rates();
}

void model_RDQ18::allocate_variables() {
  // Variable numbers
  n_variables = (prm_n_RU - 2) * 4 * 4 * 4;

  // Allocation of state_RU
  std::array<std::array<std::array<double, 4>, 4>, 4> base_RU_state;
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        base_RU_state[RU_L][RU_C][RU_R] = 0.0;
  base_RU_state[0][0][0] = 1.0;

  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    state_RU.push_back(base_RU_state);

  // Allocation of initial_state
  for (unsigned int i = 0; i < n_variables; ++i)
    initial_state.push_back(0.0);
  serialize_state(initial_state);

  // Allocation of rates_RU
  std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>
      base_RU_rates_or_flux;
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        for (RU_new = 0; RU_new < 4; ++RU_new)
          base_RU_rates_or_flux[RU_L][RU_C][RU_R][RU_new] = 0.0;

  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    rates_RU.push_back(base_RU_rates_or_flux);
  }

  // Allocation of flux_RU_*
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU) {
    flux_RU_L.push_back(base_RU_rates_or_flux);
    flux_RU_C.push_back(base_RU_rates_or_flux);
    flux_RU_R.push_back(base_RU_rates_or_flux);
  }
}

void model_RDQ18::solve_time_step(std::vector<double> &state,
                                  const double &calcium,
                                  const double &sarcomere_length,
                                  const double & /*dSL_dt*/, const double &dt) {
  // Deserialize state
  deserialize_state(state);

  // Update RU transition rates
  RU_update_rates(calcium, sarcomere_length);

  // Advance RU state
  double RU_dt = 0.0;
  double time_advanced = 0.0;
  while (time_advanced <=
         dt - 1e-10) // Cover the time-step up to a given tolerance
  {
    RU_dt = std::min(prm_time_step_update_RU_state, dt - time_advanced);
    RU_update_state(RU_dt);
    time_advanced += RU_dt;
  }

  // Re-serialize state
  serialize_state(state);
}

void model_RDQ18::deserialize_state(const std::vector<double> &state) {
  unsigned int i_current = 0;
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R) {
          state_RU[i_RU][RU_L][RU_C][RU_R] = state[i_current];
          i_current++;
        }
}

void model_RDQ18::serialize_state(std::vector<double> &state) {
  unsigned int i_current = 0;
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R) {
          state[i_current] = state_RU[i_RU][RU_L][RU_C][RU_R];
          i_current++;
        }
}

double model_RDQ18::get_active_tension(const std::vector<double> &state,
                                       const double & /*sarcomere_length*/) {
  return prm_TaMax * get_permissivity(state);
}

double model_RDQ18::get_permissivity(const std::vector<double> &state) {
  deserialize_state(state);

  double permissivity = 0;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    if (i_RU == 0) // First RU
    {
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          permissivity +=
              state_RU[0][2][RU_C][RU_R] + state_RU[0][3][RU_C][RU_R];
    } else if (i_RU == prm_n_RU - 1) // Last RU
    {
      for (RU_L = 0; RU_L < 4; ++RU_L)
        for (RU_C = 0; RU_C < 4; ++RU_C)
          permissivity += state_RU[prm_n_RU - 3][RU_L][RU_C][2] +
                          state_RU[prm_n_RU - 3][RU_L][RU_C][3];
    } else // Intermediate RUs
    {
      for (RU_L = 0; RU_L < 4; ++RU_L)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          permissivity += state_RU[i_RU - 1][RU_L][2][RU_R] +
                          state_RU[i_RU - 1][RU_L][3][RU_R];
    }
  }
  return permissivity / prm_n_RU;
}

void model_RDQ18::initialize_rates() {
  int permissive_neighbors;
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_R = 0; RU_R < 4; ++RU_R) {
        permissive_neighbors =
            permissivity_of_state[RU_L] + permissivity_of_state[RU_R];
        rates_RU[i_RU][RU_L][1][RU_R][0] = prm_Koff;
        rates_RU[i_RU][RU_L][2][RU_R][3] = prm_Koff / prm_mu;
        rates_RU[i_RU][RU_L][3][RU_R][0] =
            prm_Kbasic * std::pow(prm_gamma, 2 - permissive_neighbors);
        rates_RU[i_RU][RU_L][2][RU_R][1] =
            prm_Kbasic * std::pow(prm_gamma, 2 - permissive_neighbors);
      }
}

void model_RDQ18::RU_update_rates(const double &calcium,
                                  const double &sarcomere_length) {
  int permissive_neighbors;
  double ChiRA_i_RU, ChiLA_i_RU;
  double Q_SL = Q(sarcomere_length);
  for (i_RU = 0; i_RU < prm_n_RU; ++i_RU) {
    ChiRA_i_RU = ChiRA(sarcomere_length, i_RU);
    ChiLA_i_RU = ChiLA(sarcomere_length, i_RU);
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_R = 0; RU_R < 4; ++RU_R) {
        permissive_neighbors =
            permissivity_of_state[RU_L] + permissivity_of_state[RU_R];
        rates_RU[i_RU][RU_L][0][RU_R][1] = prm_Kon * calcium * ChiRA_i_RU;
        rates_RU[i_RU][RU_L][1][RU_R][2] =
            ChiRA_i_RU * ChiLA_i_RU *
            std::pow(prm_gamma, permissive_neighbors) * Q_SL * prm_Kbasic;
        rates_RU[i_RU][RU_L][3][RU_R][2] = prm_Kon * calcium * ChiRA_i_RU;
        rates_RU[i_RU][RU_L][0][RU_R][3] =
            ChiRA_i_RU * ChiLA_i_RU *
            std::pow(prm_gamma, permissive_neighbors) * Q_SL * prm_Kbasic /
            prm_mu;
      }
  }
}

void model_RDQ18::RU_update_state(const double &dt) {
  // Compute fluxes associated with center units
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          for (RU_new = 0; RU_new < 4; ++RU_new) {
            flux_RU_C[i_RU][RU_L][RU_C][RU_R][RU_new] =
                rates_RU[i_RU + 1][RU_L][RU_C][RU_R][RU_new] *
                state_RU[i_RU][RU_L][RU_C][RU_R];
          }

  double prob_tot;
  double rate_tot;

  // Compute fluxes associated with left units
  // --- Most left-ward triplet
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        for (RU_new = 0; RU_new < 4; ++RU_new) {
          flux_RU_L[0][RU_L][RU_C][RU_R][RU_new] =
              rates_RU[0][0][RU_L][RU_C][RU_new] *
              state_RU[0][RU_L][RU_C][RU_R];
        }

  // --- Other triplets
  for (i_RU = 1; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C) {
        prob_tot = 0.0;
        for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
          prob_tot += state_RU[i_RU - 1][RU_dummy][RU_L][RU_C];

        for (RU_new = 0; RU_new < 4; ++RU_new) {
          rate_tot = 0.0;
          if (prob_tot > 1e-12) {
            for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
              rate_tot += flux_RU_C[i_RU - 1][RU_dummy][RU_L][RU_C][RU_new];

            rate_tot /= prob_tot;
          }

          for (RU_R = 0; RU_R < 4; ++RU_R) {
            flux_RU_L[i_RU][RU_L][RU_C][RU_R][RU_new] =
                rate_tot * state_RU[i_RU][RU_L][RU_C][RU_R];
          }
        }
      }

  // Compute fluxes associated with right units
  // --- Most right-ward triplet
  for (RU_L = 0; RU_L < 4; ++RU_L)
    for (RU_C = 0; RU_C < 4; ++RU_C)
      for (RU_R = 0; RU_R < 4; ++RU_R)
        for (RU_new = 0; RU_new < 4; ++RU_new) {
          flux_RU_R[prm_n_RU - 3][RU_L][RU_C][RU_R][RU_new] =
              rates_RU[prm_n_RU - 1][RU_C][RU_R][0][RU_new] *
              state_RU[prm_n_RU - 3][RU_L][RU_C][RU_R];
        }

  // --- Other triplets
  for (i_RU = 0; i_RU < prm_n_RU - 3; ++i_RU)
    for (RU_R = 0; RU_R < 4; ++RU_R)
      for (RU_C = 0; RU_C < 4; ++RU_C) {
        prob_tot = 0.0;
        for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
          prob_tot += state_RU[i_RU + 1][RU_C][RU_R][RU_dummy];

        for (RU_new = 0; RU_new < 4; ++RU_new) {
          rate_tot = 0.0;
          if (prob_tot > 1e-12) {
            for (RU_dummy = 0; RU_dummy < 4; ++RU_dummy)
              rate_tot += flux_RU_C[i_RU + 1][RU_C][RU_R][RU_dummy][RU_new];

            rate_tot /= prob_tot;
          }

          for (RU_L = 0; RU_L < 4; ++RU_L) {
            flux_RU_R[i_RU][RU_L][RU_C][RU_R][RU_new] =
                rate_tot * state_RU[i_RU][RU_L][RU_C][RU_R];
          }
        }
      }

  // Forward Euler advance
  for (i_RU = 0; i_RU < prm_n_RU - 2; ++i_RU)
    for (RU_L = 0; RU_L < 4; ++RU_L)
      for (RU_C = 0; RU_C < 4; ++RU_C)
        for (RU_R = 0; RU_R < 4; ++RU_R)
          for (RU_new = 0; RU_new < 4; ++RU_new) {
            state_RU[i_RU][RU_L][RU_C][RU_R] +=
                dt * (flux_RU_L[i_RU][RU_new][RU_C][RU_R][RU_L] +
                      flux_RU_C[i_RU][RU_L][RU_new][RU_R][RU_C] +
                      flux_RU_R[i_RU][RU_L][RU_C][RU_new][RU_R] -
                      flux_RU_L[i_RU][RU_L][RU_C][RU_R][RU_new] -
                      flux_RU_C[i_RU][RU_L][RU_C][RU_R][RU_new] -
                      flux_RU_R[i_RU][RU_L][RU_C][RU_R][RU_new]);
          }
}


#include "RigidBodyDmp.h"
#include "QuaternionDmpModel.h"
#include "RigidBodyDmpConfig.h"
#include "Dmp.h"
#include <Eigen/Dense>

namespace dmp
{

using namespace bolero;
using namespace Eigen;
using namespace std;
using namespace dmp_cpp;
using namespace lib_manager;
DESTROY_LIB(RigidBodyDmp);
CREATE_LIB(RigidBodyDmp);

RigidBodyDmp::RigidBodyDmp(LibManager* manager) :
        LoadableBehavior(manager, "RigidBodyDmp", 1),
        manager(manager),
        translationDmp(),
        initialized(false),
        configured(false)
{
  LoadableBehavior::init(13, 13);
}

void RigidBodyDmp::setInputs(const double* values, int numInputs)
{
  assert(numInputs >= 13);
  assert(configured);
  //data format [p_x, p_y, p_z, v_x, v_y, v_z, a_x, a_y, a_z, w, x, y, z]
  translationDmp->setInputs(values, 9);
  rotationDmp->setInputs(&values[9], 4);
}

void RigidBodyDmp::getOutputs(double* values, int numOutputs) const
{
  assert(numOutputs >= 13);
  assert(configured);
  translationDmp->getOutputs(values, 9);
  rotationDmp->getOutputs(&values[9], 4);
}

void RigidBodyDmp::step()
{
  assert(configured);
  translationDmp->step();
  rotationDmp->step();
}

bool RigidBodyDmp::canStep() const
{
  return translationDmp->canStep() && rotationDmp->canStep(); // TODO
}

bool RigidBodyDmp::configure(const std::string& configPath)
{
  if(config.fromYamlFile(configPath, ""))//FIXME find way to set name
  {
    return configure(config);
  }
  else
  {
    return false;
  }
}

bool RigidBodyDmp::configureYaml(const std::string& yaml)
{
  if(config.fromYamlString(yaml, ""))//FIXME find way to set name
  {
    return configure(config);
  }
  else
  {
    return false;
  }
}

bool RigidBodyDmp::initialize(const std::string& modelPath)
{
  DMPModel model(modelPath, "");
  return initialize(model);
}

bool RigidBodyDmp::initializeYaml(const std::string yaml)
{
  DMPModel model;
  if(model.from_yaml_string(yaml, ""))//FIXME find way to set name
  {
    return initialize(model);
  }
  return false;
}

bool RigidBodyDmp::configure(const RigidBodyDmpConfig &config)
{
  if(NULL != translationDmp.get() && NULL != rotationDmp.get() && initialized)
  {
    configured = rotationDmp->configure(config.rotationConfig) && translationDmp->configure(config.translationConfig);
    return configured;
  }
  else
  {
    return false;
  }
}

bool RigidBodyDmp::initialize(const dmp_cpp::DMPModel &model)
{
  // TODO
  /*@note  Some part of the code will only work correctly
  *        if the canonical systems of both dmps are initialized
  *        using the same parameters.*/
  if(!model.is_valid())
  {
   return false;
  }
  if(model.ft_weights.size() != 6)
  {
   return false; //3 rows for the positions, 3 for the rotation
  }
  DMPModel translationModel(model);
  //keep the first 3 dimensions, they are the weights for the position
  translationModel.ft_weights.resize(3);

  QuaternionDmpModel rotationModel(model);
  rotationModel.ft_weights.resize(3);
  rotationModel.ft_weights[0] = model.ft_weights[3];
  rotationModel.ft_weights[1] = model.ft_weights[4];
  rotationModel.ft_weights[2] = model.ft_weights[5];

  if(translationModel.is_valid() && rotationModel.is_valid() && rotationModel.ft_weights.size() == 3)
  {
    translationDmp.reset(new DmpBehavior(manager));
    rotationDmp.reset(new QuaternionDmp(manager));

    initialized = rotationDmp->initialize(rotationModel) &&
            translationDmp->initialize(translationModel);
    return initialized;
  }
  else
  {
    return false;
  }
}

void RigidBodyDmp::setWeights(const double *weights, const int rows, const int cols)
{
  assert(initialized);
  assert(rows == 6);

  ArrayXXd weightsArr = Map<ArrayXXd>(const_cast<double*>(weights), rows, cols);
  translationDmp->setWeights(weightsArr.block(0, 0, 3, cols));
  rotationDmp->setWeights(weightsArr.block(3, 0, 3, cols));
}

void RigidBodyDmp::getWeights(double* weights, const int rows, const int cols)
{
  assert(initialized);
  assert(rows == 6);

  Map<ArrayXXd> weightsArr = Map<ArrayXXd>(weights, rows, cols);
  weightsArr.block(0, 0, 3, cols) = translationDmp->getWeights();
  weightsArr.block(3, 0, 3, cols) = rotationDmp->getWeights();
}

}//end namespace


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, 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 "slam-precomp.h"  // Precompiled headers

#include <mrpt/img/CEnhancedMetaFile.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/TPose2D.h>
#include <mrpt/math/distributions.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/poses/CPoint2DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/random.h>
#include <mrpt/slam/CGridMapAligner.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/tfest/se2.h>
#include <Eigen/Dense>

using namespace mrpt::math;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace mrpt::img;
using namespace mrpt::system;
using namespace mrpt::vision;
using namespace std;

CPosePDF::Ptr CGridMapAligner::AlignPDF(
	const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
	const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
	void* info)
{
	MRPT_START

	switch (options.methodSelection)
	{
		case CGridMapAligner::amCorrelation:
			return AlignPDF_correlation(
				mm1, mm2, initialEstimationPDF, runningTime, info);

		case CGridMapAligner::amModifiedRANSAC:
		case CGridMapAligner::amRobustMatch:
			// The same function has an internal switch for the specific method:
			return AlignPDF_robustMatch(
				mm1, mm2, initialEstimationPDF, runningTime, info);

		default:
			THROW_EXCEPTION("Wrong value found in 'options.methodSelection'!!");
	}

	MRPT_END
}

bool myVectorOrder(const pair<size_t, float>& o1, const pair<size_t, float>& o2)
{
	return o1.second < o2.second;
}

/*---------------------------------------------------------------
					AlignPDF_robustMatch
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch(
	const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
	const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
	void* info)
{
	MRPT_START

	ASSERT_(
		options.methodSelection == CGridMapAligner::amRobustMatch ||
		options.methodSelection == CGridMapAligner::amModifiedRANSAC);

	TReturnInfo outInfo;
	mrpt::tfest::TMatchingPairList& correspondences =
		outInfo.correspondences;  // Use directly this placeholder to save 1
	// variable & 1 copy.
	mrpt::tfest::TMatchingPairList largestConsensusCorrs;

	CTicTac* tictac = nullptr;

	CPose2D grossEst = initialEstimationPDF.mean;

	CLandmarksMap::Ptr lm1(new CLandmarksMap());
	CLandmarksMap::Ptr lm2(new CLandmarksMap());

	std::vector<size_t> idxs1, idxs2;
	std::vector<size_t>::iterator it1, it2;

	// Asserts:
	// -----------------
	const CMultiMetricMap* multimap1 = nullptr;
	const CMultiMetricMap* multimap2 = nullptr;
	const COccupancyGridMap2D* m1 = nullptr;
	const COccupancyGridMap2D* m2 = nullptr;

	if (IS_CLASS(*mm1, CMultiMetricMap) && IS_CLASS(*mm2, CMultiMetricMap))
	{
		multimap1 = static_cast<const CMultiMetricMap*>(mm1);
		multimap2 = static_cast<const CMultiMetricMap*>(mm2);

		ASSERT_(multimap1->countMapsByClass<COccupancyGridMap2D>());
		ASSERT_(multimap2->countMapsByClass<COccupancyGridMap2D>());

		m1 = multimap1->mapByClass<COccupancyGridMap2D>().get();
		m2 = multimap2->mapByClass<COccupancyGridMap2D>().get();
	}
	else if (
		IS_CLASS(*mm1, COccupancyGridMap2D) &&
		IS_CLASS(*mm2, COccupancyGridMap2D))
	{
		m1 = static_cast<const COccupancyGridMap2D*>(mm1);
		m2 = static_cast<const COccupancyGridMap2D*>(mm2);
	}
	else
		THROW_EXCEPTION(
			"Metric maps must be of classes COccupancyGridMap2D or "
			"CMultiMetricMap");

	ASSERT_(m1);
	ASSERT_(m2);

	ASSERTMSG_(
		m1->getResolution() == m2->getResolution(),
		mrpt::format(
			"Different resolutions for m1, m2:\n"
			"\tres(m1) = %f\n\tres(m2) = %f\n",
			m1->getResolution(), m2->getResolution()));

	// The algorithm output auxiliar info:
	// -------------------------------------------------
	outInfo.goodness = 1.0f;

	outInfo.landmarks_map1 = lm1;
	outInfo.landmarks_map2 = lm2;

	// The PDF to estimate:
	// ------------------------------------------------------
	CPosePDFSOG::Ptr pdf_SOG = std::make_shared<CPosePDFSOG>();

	// Extract features from grid-maps:
	// ------------------------------------------------------
	const size_t N1 =
		std::max(40, mrpt::round(m1->getArea() * options.featsPerSquareMeter));
	const size_t N2 =
		std::max(40, mrpt::round(m2->getArea() * options.featsPerSquareMeter));

	m_grid_feat_extr.extractFeatures(
		*m1, *lm1, N1, options.feature_descriptor,
		options.feature_detector_options);
	m_grid_feat_extr.extractFeatures(
		*m2, *lm2, N2, options.feature_descriptor,
		options.feature_detector_options);

	if (runningTime)
	{
		tictac = new CTicTac();
		tictac->Tic();
	}

	const size_t nLM1 = lm1->size();
	const size_t nLM2 = lm2->size();

	//  At least two landmarks at each map!
	// ------------------------------------------------------
	if (nLM1 < 2 || nLM2 < 2)
	{
		outInfo.goodness = 0;
	}
	else
	{
		//#define METHOD_FFT
		//#define DEBUG_SHOW_CORRELATIONS

		// Compute correlation between landmarks:
		// ---------------------------------------------
		CMatrixFloat CORR(lm1->size(), lm2->size()), auxCorr;
		CImage im1, im2;  // Grayscale
		CVectorFloat corr;
		unsigned int corrsCount = 0;
		std::vector<bool> hasCorr(nLM1, false);

		const double thres_max = options.threshold_max;
		const double thres_delta = options.threshold_delta;

		// CDisplayWindowPlots::Ptr	auxWin;
		if (options.debug_show_corrs)
		{
			// auxWin = CDisplayWindowPlots::Ptr( new
			// CDisplayWindowPlots("Individual corr.") );
			std::cerr << "Warning: options.debug_show_corrs has no effect "
						 "since MRPT 0.9.1\n";
		}

		for (size_t idx1 = 0; idx1 < nLM1; idx1++)
		{
			// CVectorFloat  	corrs_indiv;
			vector<pair<size_t, float>> corrs_indiv;  // (index, distance);
			// Index is used to
			// recover the original
			// index after sorting.
			vector<float> corrs_indiv_only;
			corrs_indiv.reserve(nLM2);
			corrs_indiv_only.reserve(nLM2);

			for (size_t idx2 = 0; idx2 < nLM2; idx2++)
			{
				float minDist;
				minDist =
					lm1->landmarks.get(idx1)->features[0].descriptorDistanceTo(
						lm2->landmarks.get(idx2)->features[0]);

				corrs_indiv.emplace_back(idx2, minDist);
				corrs_indiv_only.push_back(minDist);
			}  // end for idx2

			// double corr_mean,corr_std;
			// mrpt::math::meanAndStd(corrs_indiv_only,corr_mean,corr_std);
			const double corr_best = mrpt::math::minimum(corrs_indiv_only);
			// cout << "M: " << corr_mean << " std: " << corr_std << " best: "
			// << corr_best << endl;

			// Sort the list and keep the N best features:
			std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder);

			// const size_t nBestToKeep = std::min( (size_t)30,
			// corrs_indiv.size() );
			const size_t nBestToKeep = corrs_indiv.size();

			for (size_t w = 0; w < nBestToKeep; w++)
			{
				if (corrs_indiv[w].second <= thres_max &&
					corrs_indiv[w].second <= (corr_best + thres_delta))
				{
					idxs1.push_back(idx1);
					idxs2.push_back(corrs_indiv[w].first);
					outInfo.correspondences_dists_maha.emplace_back(
						idx1, corrs_indiv[w].first, corrs_indiv[w].second);
				}
			}
		}  // end for idx1

		// Save an image with each feature and its matches:
		if (options.save_feat_coors)
		{
			mrpt::system::deleteFilesInDirectory("grid_feats");
			mrpt::system::createDirectory("grid_feats");

			std::map<size_t, std::set<size_t>> corrs_by_idx;
			for (size_t l = 0; l < idxs1.size(); l++)
				corrs_by_idx[idxs1[l]].insert(idxs2[l]);

			for (auto& it : corrs_by_idx)
			{
				CMatrixFloat descriptor1;
				lm1->landmarks.get(it.first)
					->features[0]
					.getFirstDescriptorAsMatrix(descriptor1);

				im1.setFromMatrix(descriptor1, true /*normalized*/);

				const size_t FEAT_W = im1.getWidth();
				const size_t FEAT_H = im1.getHeight();
				const size_t nF = it.second.size();

				CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF);
				img_compose.filledRectangle(
					0, 0, img_compose.getWidth() - 1,
					img_compose.getHeight() - 1, TColor::black());

				img_compose.drawImage(5, 5, im1);

				size_t j;
				std::set<size_t>::iterator it_j;
				string fil =
					format("grid_feats/_FEAT_MATCH_%03i", (int)it.first);

				for (j = 0, it_j = it.second.begin(); j < nF; ++j, ++it_j)
				{
					fil += format("_%u", static_cast<unsigned int>(*it_j));

					CMatrixFloat descriptor2;
					lm2->landmarks.get(*it_j)
						->features[0]
						.getFirstDescriptorAsMatrix(descriptor2);
					im2.setFromMatrix(descriptor2, true);
					img_compose.drawImage(
						10 + FEAT_W, 5 + j * (FEAT_H + 5), im2);
				}
				fil += ".png";
				img_compose.saveToFile(fil);
			}  // end for map
		}

		// ------------------------------------------------------------------
		// Create the list of correspondences from the lists: idxs1 & idxs2
		// ------------------------------------------------------------------
		correspondences.clear();
		for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end();
			 ++it1, ++it2)
		{
			mrpt::tfest::TMatchingPair mp;
			mp.this_idx = *it1;
			mp.this_x = lm1->landmarks.get(*it1)->pose_mean.x;
			mp.this_y = lm1->landmarks.get(*it1)->pose_mean.y;
			mp.this_z = lm1->landmarks.get(*it1)->pose_mean.z;

			mp.other_idx = *it2;
			mp.other_x = lm2->landmarks.get(*it2)->pose_mean.x;
			mp.other_y = lm2->landmarks.get(*it2)->pose_mean.y;
			mp.other_z = lm2->landmarks.get(*it2)->pose_mean.z;
			correspondences.push_back(mp);

			if (!hasCorr[*it1])
			{
				hasCorr[*it1] = true;
				corrsCount++;
			}
		}  // end for corres.

		outInfo.goodness = (2.0f * corrsCount) / (nLM1 + nLM2);

		// Compute the estimation using ALL the correspondences (NO ROBUST):
		// ----------------------------------------------------------------------
		mrpt::math::TPose2D noRobustEst;
		if (!mrpt::tfest::se2_l2(correspondences, noRobustEst))
		{
			// There's no way to match the maps! e.g. no correspondences
			outInfo.goodness = 0;
			pdf_SOG->clear();
			outInfo.sog1 = pdf_SOG;
			outInfo.sog2 = pdf_SOG;
			outInfo.sog3 = pdf_SOG;
		}
		else
		{
			outInfo.noRobustEstimation = mrpt::poses::CPose2D(noRobustEst);
			MRPT_LOG_INFO(mrpt::format(
				"[CGridMapAligner] Overall estimation(%u corrs, total: "
				"%u): (%.03f,%.03f,%.03fdeg)\n",
				corrsCount, (unsigned)correspondences.size(),
				outInfo.noRobustEstimation.x(), outInfo.noRobustEstimation.y(),
				RAD2DEG(outInfo.noRobustEstimation.phi())));

			// The list of SOG modes & their corresponding sub-sets of
			// matchings:
			using TMapMatchingsToPoseMode = std::map<
				mrpt::tfest::TMatchingPairList, CPosePDFSOG::TGaussianMode>;
			TMapMatchingsToPoseMode sog_modes;

			// ---------------------------------------------------------------
			// Now, we have to choose between the methods:
			//  - CGridMapAligner::amRobustMatch  ("normal" RANSAC)
			//  - CGridMapAligner::amModifiedRANSAC
			// ---------------------------------------------------------------
			if (options.methodSelection == CGridMapAligner::amRobustMatch)
			{
				// ====================================================
				//             METHOD: "Normal" RANSAC
				// ====================================================

				// RANSAC over the found correspondences:
				// -------------------------------------------------
				const unsigned int min_inliers =
					options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2;

				mrpt::tfest::TSE2RobustParams tfest_params;
				tfest_params.ransac_minSetSize = min_inliers;
				tfest_params.ransac_maxSetSize = nLM1 + nLM2;
				tfest_params.ransac_mahalanobisDistanceThreshold =
					options.ransac_mahalanobisDistanceThreshold;
				tfest_params.ransac_nSimulations = 0;  // 0=auto
				tfest_params.ransac_fuseByCorrsMatch = true;
				tfest_params.ransac_fuseMaxDiffXY = 0.01;
				tfest_params.ransac_fuseMaxDiffPhi = DEG2RAD(0.1);
				tfest_params.ransac_algorithmForLandmarks = true;
				tfest_params.probability_find_good_model =
					options.ransac_prob_good_inliers;
				tfest_params.verbose = false;

				mrpt::tfest::TSE2RobustResult tfest_result;
				mrpt::tfest::se2_l2_robust(
					correspondences, options.ransac_SOG_sigma_m, tfest_params,
					tfest_result);

				ASSERT_(pdf_SOG);
				*pdf_SOG = tfest_result.transformation;
				largestConsensusCorrs = tfest_result.largestSubSet;

				// Simplify the SOG by merging close modes:
				// -------------------------------------------------
				size_t nB = pdf_SOG->size();
				outInfo.sog1 = pdf_SOG;

				// Keep only the most-likely Gaussian mode:
				CPosePDFSOG::TGaussianMode best_mode;
				best_mode.log_w = -std::numeric_limits<double>::max();

				for (size_t n = 0; n < pdf_SOG->size(); n++)
				{
					const CPosePDFSOG::TGaussianMode& m = (*pdf_SOG)[n];

					if (m.log_w > best_mode.log_w) best_mode = m;
				}

				pdf_SOG->clear();
				pdf_SOG->push_back(best_mode);
				outInfo.sog2 = pdf_SOG;

				MRPT_LOG_INFO_STREAM(
					"[CGridMapAligner] amRobustMatch: "
					<< nB << " SOG modes reduced to " << pdf_SOG->size()
					<< " (most-likely) (min.inliers=" << min_inliers << ")\n");

			}  // end of amRobustMatch
			else
			{
				// ====================================================
				//             METHOD: "Modified" RANSAC
				// ====================================================
				mrpt::tfest::TMatchingPairList all_corrs = correspondences;

				const size_t nCorrs = all_corrs.size();
				ASSERT_(nCorrs >= 2);

				pdf_SOG->clear();  // Start with 0 Gaussian modes

				// Build a points-map for exploiting its KD-tree:
				// ----------------------------------------------------
				CSimplePointsMap lm1_pnts, lm2_pnts;
				lm1_pnts.reserve(nLM1);
				for (size_t i = 0; i < nLM1; i++)
					lm1_pnts.insertPoint(lm1->landmarks.get(i)->pose_mean);
				lm2_pnts.reserve(nLM2);
				for (size_t i = 0; i < nLM2; i++)
					lm2_pnts.insertPoint(lm2->landmarks.get(i)->pose_mean);

				// RANSAC loop
				// ---------------------
				const size_t minInliersTOaccept =
					round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2));
				// Set an initial # of iterations:
				const unsigned int ransac_min_nSimulations =
					2 * (nLM1 + nLM2);  // 1000;
				unsigned int ransac_nSimulations =
					10;  // It doesn't matter actually, since will be changed in
				// the first loop
				const double probability_find_good_model = 0.9999;

				const double chi2_thres_dim1 =
					mrpt::math::chi2inv(options.ransac_chi2_quantile, 1);
				const double chi2_thres_dim2 =
					mrpt::math::chi2inv(options.ransac_chi2_quantile, 2);

				// Generic 2x2 covariance matrix for all features in their local
				// coords:
				CMatrixDouble22 COV_pnt;
				COV_pnt(0, 0) = COV_pnt(1, 1) =
					square(options.ransac_SOG_sigma_m);

				// The absolute number of trials.
				// in practice it's only important for a reduced number of
				// correspondences, to avoid a dead-lock:
				//  It's the binomial coefficient:
				//   / n \      n!          n * (n-1)
				//   |   | = ----------- = -----------
				//   \ 2 /    2! (n-2)!         2
				//
				const unsigned int max_trials =
					(nCorrs * (nCorrs - 1) / 2) *
					5;  // "*5" is just for safety...

				unsigned int iter = 0;  // Valid iterations (those passing the
				// first mahalanobis test)
				unsigned int trials = 0;  // counter of all iterations,
				// including "iter" + failing ones.
				while (iter < ransac_nSimulations &&
					   trials <
						   max_trials)  // ransac_nSimulations can be dynamic
				{
					trials++;

					mrpt::tfest::TMatchingPairList tentativeSubSet;

					// Pick 2 random correspondences:
					uint32_t idx1, idx2;
					idx1 = getRandomGenerator().drawUniform32bit() % nCorrs;
					do
					{
						idx2 = getRandomGenerator().drawUniform32bit() % nCorrs;
					} while (idx1 == idx2);  // Avoid a degenerated case!

					// Uniqueness of features:
					if (all_corrs[idx1].this_idx == all_corrs[idx2].this_idx ||
						all_corrs[idx1].this_idx == all_corrs[idx2].other_idx)
						continue;
					if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx ||
						all_corrs[idx1].other_idx == all_corrs[idx2].other_idx)
						continue;

					// Check the feasibility of this pair "idx1"-"idx2":
					//  The distance between the pair of points in MAP1 must be
					//  very close
					//   to that of their correspondences in MAP2:
					const double corrs_dist1 =
						mrpt::math::distanceBetweenPoints(
							all_corrs[idx1].this_x, all_corrs[idx1].this_y,
							all_corrs[idx1].this_z, all_corrs[idx2].this_x,
							all_corrs[idx2].this_y, all_corrs[idx2].this_z);

					const double corrs_dist2 =
						mrpt::math::distanceBetweenPoints(
							all_corrs[idx1].other_x, all_corrs[idx1].other_y,
							all_corrs[idx1].other_z, all_corrs[idx2].other_x,
							all_corrs[idx2].other_y, all_corrs[idx2].other_z);

					// Is is a consistent possibility?
					//  We use a chi2 test (see paper for the derivation)
					const double corrs_dist_chi2 =
						square(square(corrs_dist1) - square(corrs_dist2)) /
						(8.0 * square(options.ransac_SOG_sigma_m) *
						 (square(corrs_dist1) + square(corrs_dist2)));

					if (corrs_dist_chi2 > chi2_thres_dim1) continue;  // Nope

					iter++;  // Do not count iterations if they fail the test
					// above.

					// before proceeding with this hypothesis, is it an old one?
					bool is_new_hyp = true;
					for (auto& sog_mode : sog_modes)
					{
						if (sog_mode.first.contains(all_corrs[idx1]) &&
							sog_mode.first.contains(all_corrs[idx2]))
						{
							// Increment weight:
							sog_mode.second.log_w =
								std::log(std::exp(sog_mode.second.log_w) + 1.0);
							is_new_hyp = false;
							break;
						}
					}
					if (!is_new_hyp) continue;

					// Ok, it's a new hypothesis:
					tentativeSubSet.push_back(all_corrs[idx1]);
					tentativeSubSet.push_back(all_corrs[idx2]);

					// Maintain a list of already used landmarks IDs in both
					// maps to avoid repetitions:
					std::vector<bool> used_landmarks1(nLM1, false);
					std::vector<bool> used_landmarks2(nLM2, false);

					used_landmarks1[all_corrs[idx1].this_idx] = true;
					used_landmarks1[all_corrs[idx2].this_idx] = true;
					used_landmarks2[all_corrs[idx1].other_idx] = true;
					used_landmarks2[all_corrs[idx2].other_idx] = true;

					// Build the transformation for these temptative
					// correspondences:
					bool keep_incorporating = true;
					CPosePDFGaussian temptPose;
					do  // Incremently incorporate inliers:
					{
						if (!mrpt::tfest::se2_l2(tentativeSubSet, temptPose))
							continue;  // Invalid matching...

						// The computed cov is "normalized", i.e. must be
						// multiplied by std^2_xy
						temptPose.cov *= square(options.ransac_SOG_sigma_m);

						// cout << "q: " << temptPose << endl;

						// Find the landmark in MAP2 with the best (maximum)
						// product-integral:
						//   (i^* , j^*) = arg max_(i,j) \int p_i()p_j()
						//----------------------------------------------------------------------
						const double ccos = cos(temptPose.mean.phi());
						const double ssin = sin(temptPose.mean.phi());

						CMatrixDouble22 Hc;  // Jacobian wrt point_j
						Hc(1, 1) = ccos;
						Hc(0, 0) = ccos;
						Hc(1, 0) = ssin;
						Hc(0, 1) = -ssin;

						CMatrixFixed<double, 2, 3>
							Hq;  // Jacobian wrt transformation q
						Hq(0, 0) = 1;
						Hq(1, 1) = 1;

						TPoint2D p2_j_local;
						vector<float> matches_dist;
						std::vector<size_t> matches_idx;

						CPoint2DPDFGaussian pdf_M2_j;
						CPoint2DPDFGaussian pdf_M1_i;

// Use integral-of-product vs. mahalanobis distances to match:
#define GRIDMAP_USE_PROD_INTEGRAL

#ifdef GRIDMAP_USE_PROD_INTEGRAL
						double best_pair_value = 0;
#else
						double best_pair_value =
							std::numeric_limits<double>::max();
#endif
						double best_pair_d2 =
							std::numeric_limits<double>::max();
						pair<size_t, size_t> best_pair_ij;

						//#define SHOW_CORRS

#ifdef SHOW_CORRS
						CDisplayWindowPlots win("Matches");
#endif
						for (size_t j = 0; j < nLM2; j++)
						{
							if (used_landmarks2[j]) continue;

							lm2_pnts.getPoint(
								j, p2_j_local);  // In local coords.
							pdf_M2_j.mean = mrpt::poses::CPoint2D(
								temptPose.mean +
								p2_j_local);  // In (temptative) global coords:
							pdf_M2_j.cov(0, 0) = pdf_M2_j.cov(1, 1) =
								square(options.ransac_SOG_sigma_m);

#ifdef SHOW_CORRS
							win.plotEllipse(
								pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
								pdf_M2_j.cov, 2, "b-",
								format("M2_%u", (unsigned)j), true);
#endif

							static const unsigned int N_KDTREE_SEARCHED = 3;

							// Look for a few close features which may be
							// potential matches:
							lm1_pnts.kdTreeNClosestPoint2DIdx(
								pdf_M2_j.mean.x(), pdf_M2_j.mean.y(),
								N_KDTREE_SEARCHED, matches_idx, matches_dist);

							// And for each one, compute the product-integral:
							for (unsigned long u : matches_idx)
							{
								if (used_landmarks1[u]) continue;

								// Jacobian wrt transformation q
								Hq(0, 2) =
									-p2_j_local.x * ssin - p2_j_local.y * ccos;
								Hq(1, 2) =
									p2_j_local.x * ccos - p2_j_local.y * ssin;

								// COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t
								pdf_M1_i.cov =
									mrpt::math::multiply_HCHt(Hc, COV_pnt);
								pdf_M1_i.cov += mrpt::math::multiply_HCHt(
									Hq, temptPose.cov);

								float px, py;
								lm1_pnts.getPoint(u, px, py);
								pdf_M1_i.mean.x(px);
								pdf_M1_i.mean.y(py);

#ifdef SHOW_CORRS
								win.plotEllipse(
									pdf_M1_i.mean.x(), pdf_M1_i.mean.y(),
									pdf_M1_i.cov, 2, "r-",
									format("M1_%u", (unsigned)matches_idx[u]),
									true);
#endif

// And now compute the product integral:
#ifdef GRIDMAP_USE_PROD_INTEGRAL
								const double prod_ij =
									pdf_M1_i.productIntegralWith(pdf_M2_j);
								// const double prod_ij_d2 = square(
								// pdf_M1_i.mahalanobisDistanceTo(pdf_M2_j) );

								if (prod_ij > best_pair_value)
#else
								const double prod_ij =
									pdf_M1_i.mean.distanceTo(pdf_M2_j.mean);
								if (prod_ij < best_pair_value)
#endif
								{
									// const double prodint_ij =
									// pdf_M1_i.productIntegralWith2D(pdf_M2_j);

									best_pair_value = prod_ij;
									best_pair_ij.first = u;
									best_pair_ij.second = j;

									best_pair_d2 =
										square(pdf_M1_i.mahalanobisDistanceTo(
											pdf_M2_j));

									// cout << "P1: " << pdf_M1_i.mean << " C= "
									// << pdf_M1_i.cov.inMatlabFormat() << endl;
									// cout << "P2: " << pdf_M2_j.mean << " C= "
									// << pdf_M2_j.cov.inMatlabFormat() << endl;
									// cout << "  -> " << format("%e",prod_ij)
									// << " d2: " << best_pair_d2 << endl <<
									// endl;
								}
							}  // end for u (closest matches of LM2 in MAP 1)

#ifdef SHOW_CORRS
							win.axis_fit(true);
							win.waitForKey();
							win.clear();
#endif

						}  // end for each LM2

						// Stop when the best choice has a bad mahal. dist.
						keep_incorporating = false;

						// For the best (i,j), gate by the mahalanobis distance:
						if (best_pair_d2 < chi2_thres_dim2)
						{
							// AND, also, check if the descriptors have some
							// resemblance!!
							// ----------------------------------------------------------------
							// double feat_dist =
							// lm1->landmarks.get(best_pair_ij.first)->features[0]->descriptorDistanceTo(*lm1->landmarks.get(best_pair_ij.second)->features[0]);
							// if (feat_dist< options.threshold_max)
							{
								float p1_i_localx, p1_i_localy;
								float p2_j_localx, p2_j_localy;
								lm1_pnts.getPoint(
									best_pair_ij.first, p1_i_localx,
									p1_i_localy);
								lm2_pnts.getPoint(
									best_pair_ij.second, p2_j_localx,
									p2_j_localy);  // In local coords.

								used_landmarks1[best_pair_ij.first] = true;
								used_landmarks2[best_pair_ij.second] = true;

								tentativeSubSet.push_back(
									mrpt::tfest::TMatchingPair(
										best_pair_ij.first, best_pair_ij.second,
										p1_i_localx, p1_i_localy, 0,  // MAP1
										p2_j_localx, p2_j_localy, 0  // MAP2
										));

								keep_incorporating = true;
							}
						}

					} while (keep_incorporating);

					// Consider this pairing?
					const size_t ninliers = tentativeSubSet.size();
					if (ninliers > minInliersTOaccept)
					{
						CPosePDFSOG::TGaussianMode newGauss;
						newGauss.log_w = 0;  // log(1);  //
						// std::log(static_cast<double>(nCoincidences));
						newGauss.mean = temptPose.mean;
						newGauss.cov = temptPose.cov;

						sog_modes[tentativeSubSet] = newGauss;

						// cout << "ITER: " << iter << " #inliers: " << ninliers
						// << " q: " << temptPose.mean << endl;
					}

					// Keep the largest consensus & dynamic # of steps:
					if (largestConsensusCorrs.size() < ninliers)
					{
						largestConsensusCorrs = tentativeSubSet;

						// Update estimate of N, the number of trials to ensure
						// we pick,
						// with probability p, a data set with no outliers.
						const double fracinliers =
							ninliers /
							static_cast<double>(std::min(nLM1, nLM2));
						double pNoOutliers =
							1 - pow(fracinliers,
									static_cast<double>(
										2.0));  // minimumSizeSamplesToFit

						pNoOutliers = std::max(
							std::numeric_limits<double>::epsilon(),
							pNoOutliers);  // Avoid division by -Inf
						pNoOutliers = std::min(
							1.0 - std::numeric_limits<double>::epsilon(),
							pNoOutliers);  // Avoid division by 0.
						// Number of
						ransac_nSimulations =
							log(1 - probability_find_good_model) /
							log(pNoOutliers);

						if (ransac_nSimulations < ransac_min_nSimulations)
							ransac_nSimulations = ransac_min_nSimulations;

						// if (verbose)
						//	cout << "[Align] Iter #" << iter << " Est. #iters: "
						//<< ransac_nSimulations << "  pNoOutliers=" <<
						// pNoOutliers << " #inliers: " << ninliers << endl;
					}

				}  // end of RANSAC loop

				// Move SOG modes into pdf_SOG:
				pdf_SOG->clear();
				for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
				{
					cout << "SOG mode: " << s->second.mean
						 << " inliers: " << s->first.size() << endl;
					pdf_SOG->push_back(s->second);
				}

				// Normalize:
				if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights();

				// Simplify the SOG by merging close modes:
				// -------------------------------------------------
				size_t nB = pdf_SOG->size();
				outInfo.sog1 = pdf_SOG;

				CTicTac merge_clock;
				pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
				const double merge_clock_tim = merge_clock.Tac();

				outInfo.sog2 = pdf_SOG;
				size_t nA = pdf_SOG->size();
				MRPT_LOG_INFO(mrpt::format(
					"[CGridMapAligner] amModifiedRANSAC: %u SOG modes "
					"merged to %u in %.03fsec\n",
					(unsigned int)nB, (unsigned int)nA, merge_clock_tim));

			}  // end of amModifiedRANSAC

			// Save best corrs:
			if (options.debug_save_map_pairs)
			{
				static unsigned int NN = 0;
				static const COccupancyGridMap2D* lastM1 = nullptr;
				if (lastM1 != m1)
				{
					lastM1 = m1;
					NN = 0;
				}
				printf(
					"   Largest consensus: %u\n",
					static_cast<unsigned>(largestConsensusCorrs.size()));
				CEnhancedMetaFile::LINUX_IMG_WIDTH(
					m1->getSizeX() + m2->getSizeX() + 50);
				CEnhancedMetaFile::LINUX_IMG_HEIGHT(
					max(m1->getSizeY(), m2->getSizeY()) + 50);

				for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s)
				{
					COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences(
						format("__debug_corrsGrid_%05u.emf", NN), m1, m2,
						s->first);
					COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences(
						format("__debug_corrsGrid_%05u.png", NN), m1, m2,
						s->first);
					++NN;
				}
			}

			// --------------------------------------------------------------------
			// At this point:
			//   - "pdf_SOG": has the resulting PDF with the SOG (from whatever
			//   method)
			//   - "largestConsensusCorrs": The 'best' set of correspondences
			//
			//  Now: If we had a multi-metric map, use the points map to improve
			//        the estimation with ICP.
			// --------------------------------------------------------------------
			if (multimap1 && multimap2 &&
				multimap1->countMapsByClass<CSimplePointsMap>() != 0 &&
				multimap2->countMapsByClass<CSimplePointsMap>() != 0)
			{
				auto pnts1 = multimap1->mapByClass<CSimplePointsMap>();
				auto pnts2 = multimap2->mapByClass<CSimplePointsMap>();

				CICP icp;
				CICP::TReturnInfo icpInfo;

				icp.options.maxIterations = 20;
				icp.options.smallestThresholdDist = 0.05f;
				icp.options.thresholdDist = 0.75f;

				// Invoke ICP once for each mode in the SOG:
				size_t cnt = 0;
				outInfo.icp_goodness_all_sog_modes.clear();
				for (auto i = pdf_SOG->begin(); i != pdf_SOG->end(); ++cnt)
				{
					CPosePDF::Ptr icp_est = icp.Align(
						pnts1.get(), pnts2.get(), (i)->mean, nullptr, &icpInfo);

					//(i)->cov(0,0) += square( 0.05 );
					//(i)->cov(1,1) += square( 0.05 );
					//(i)->cov(2,2) += square( DEG2RAD(0.05) );

					CPosePDFGaussian i_gauss(i->mean, i->cov);
					CPosePDFGaussian icp_gauss(icp_est->getMeanVal(), i->cov);

					const double icp_maha_dist =
						i_gauss.mahalanobisDistanceTo(icp_gauss);

					cout << "ICP " << cnt << " log-w: " << i->log_w
						 << " Goodness: " << 100 * icpInfo.goodness
						 << "  D_M= " << icp_maha_dist << endl;
					cout << "  final pos: " << icp_est->getMeanVal() << endl;
					cout << "    org pos: " << i->mean << endl;

					outInfo.icp_goodness_all_sog_modes.push_back(
						icpInfo.goodness);

					// Discard?
					if (icpInfo.goodness > options.min_ICP_goodness &&
						icp_maha_dist < options.max_ICP_mahadist)
					{
						icp_est->getMean((i)->mean);
						++i;
					}
					else
					{
						// Delete:
						i = pdf_SOG->erase(i);
					}

				}  // end for i

				// Merge:
				outInfo.sog3 = pdf_SOG;

				pdf_SOG->mergeModes(options.maxKLd_for_merge, false);
				MRPT_LOG_DEBUG_STREAM(
					"[CGridMapAligner] " << pdf_SOG->size()
										 << " SOG modes merged after ICP.");

			}  // end multimapX

		}  // end of, yes, we have enough correspondences

	}  // end of: yes, there are landmarks in the grid maps!

	// Copy the output info if requested:
	// -------------------------------------------------
	MRPT_TODO(
		"Refactor `info` so it is polymorphic and can use dynamic_cast<> here");
	if (info)
	{
		auto* info_ = static_cast<TReturnInfo*>(info);
		*info_ = outInfo;
	}

	if (runningTime)
	{
		*runningTime = tictac->Tac();
		delete tictac;
	}

	return pdf_SOG;

	MRPT_END
}

/*---------------------------------------------------------------
					AlignPDF_correlation
---------------------------------------------------------------*/
CPosePDF::Ptr CGridMapAligner::AlignPDF_correlation(
	const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2,
	const CPosePDFGaussian& initialEstimationPDF, float* runningTime,
	void* info)
{
	MRPT_UNUSED_PARAM(initialEstimationPDF);
	MRPT_UNUSED_PARAM(info);

	MRPT_START

	//#define	CORRELATION_SHOW_DEBUG

	CTicTac* tictac = nullptr;

	// Asserts:
	// -----------------
	ASSERT_(mm1->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
	ASSERT_(mm2->GetRuntimeClass() == CLASS_ID(COccupancyGridMap2D));
	const auto* m1 = static_cast<const COccupancyGridMap2D*>(mm1);
	const auto* m2 = static_cast<const COccupancyGridMap2D*>(mm2);

	ASSERT_(m1->getResolution() == m2->getResolution());

	if (runningTime)
	{
		tictac = new CTicTac();
		tictac->Tic();
	}

	// The PDF to estimate:
	// ------------------------------------------------------
	CPosePDFGaussian::Ptr PDF = std::make_shared<CPosePDFGaussian>();

	// Determine the extension to compute the correlation into:
	// ----------------------------------------------------------
	float phiResolution = DEG2RAD(0.2f);
	float phiMin = -M_PIf + 0.5f * phiResolution;
	float phiMax = M_PIf;

	// Compute the difference between maps for each (u,v) pair!
	// --------------------------------------------------------------
	float phi;
	float pivotPt_x = 0.5f * (m1->getXMax() + m1->getXMin());
	float pivotPt_y = 0.5f * (m1->getYMax() + m1->getYMin());
	COccupancyGridMap2D map2_mod;
	CImage map1_img, map2_img;
	float currentMaxCorr = -1e6f;

	m1->getAsImage(map1_img);

	map2_mod.setSize(
		m1->getXMin(), m1->getXMax(), m1->getYMin(), m1->getYMax(),
		m1->getResolution());
	size_t map2_lx = map2_mod.getSizeX();
	size_t map2_ly = map2_mod.getSizeY();
	CMatrixF outCrossCorr, bestCrossCorr;
	float map2width_2 = 0.5f * (map2_mod.getXMax() - map2_mod.getXMin());

#ifdef CORRELATION_SHOW_DEBUG
	CDisplayWindow* win = new CDisplayWindow("corr");
	CDisplayWindow* win2 = new CDisplayWindow("map2_mod");
#endif

	// --------------------------------------------------------
	// Use FFT-based correlation for each orientation:
	// --------------------------------------------------------
	for (phi = phiMin; phi < phiMax; phi += phiResolution)
	{
		// Create the displaced map2 grid, for the size of map1:
		// --------------------------------------------------------
		CPose2D v2(
			pivotPt_x - cos(phi) * map2width_2,
			pivotPt_y - sin(phi) * map2width_2,
			phi);  // Rotation point: the centre of img1:
		CPoint2D v1, v3;
		v2 = CPose2D(0, 0, 0) - v2;  // Inverse

		for (unsigned int cy2 = 0; cy2 < map2_ly; cy2++)
		{
			COccupancyGridMap2D::cellType* row = map2_mod.getRow(cy2);
			for (unsigned int cx2 = 0; cx2 < map2_lx; cx2++)
			{
				v3 = v2 + CPoint2D(map2_mod.idx2x(cx2), map2_mod.idx2y(cy2));
				*row++ = m2->p2l(m2->getPos(v3.x(), v3.y()));
			}
		}

		map2_mod.getAsImage(map2_img);
		//		map2_img.saveToBMP("__debug_map2mod.bmp");

		// Compute the correlation:
		map1_img.cross_correlation_FFT(
			map2_img, outCrossCorr, -1, -1, -1, -1,
			127,  // Bias to be substracted
			127  // Bias to be substracted
		);

		float corrPeak = outCrossCorr.maxCoeff();
		printf("phi = %fdeg \tmax corr=%f\n", RAD2DEG(phi), corrPeak);

		// Keep the maximum:
		if (corrPeak > currentMaxCorr)
		{
			currentMaxCorr = corrPeak;
			bestCrossCorr = outCrossCorr;
			PDF->mean.phi(phi);
		}

#ifdef CORRELATION_SHOW_DEBUG
		outCrossCorr.adjustRange(0, 1);
		CImage aux(outCrossCorr, true);
		win->showImage(aux);
		win2->showImage(map2_img);
		std::this_thread::sleep_for(5ms);
#endif

	}  // end for phi

	if (runningTime)
	{
		*runningTime = tictac->Tac();
		delete tictac;
	}

	CImage aux;
	aux.setFromMatrix(bestCrossCorr, false /* do normalization [0,1]*/);
	aux.saveToFile("_debug_best_corr.png");

#ifdef CORRELATION_SHOW_DEBUG
	delete win;
	delete win2;
#endif

	// Transform the best corr matrix peak into coordinates:
	std::size_t uMax, vMax;
	currentMaxCorr = bestCrossCorr.maxCoeff(uMax, vMax);

	PDF->mean.x(m1->idx2x(uMax));
	PDF->mean.y(m1->idx2y(vMax));

	return PDF;

	// Done!
	MRPT_END
}

/*---------------------------------------------------------------
					TConfigParams
  ---------------------------------------------------------------*/
CGridMapAligner::TConfigParams::TConfigParams() : feature_detector_options() {}
/*---------------------------------------------------------------
					dumpToTextStream
  ---------------------------------------------------------------*/
void CGridMapAligner::TConfigParams::dumpToTextStream(std::ostream& out) const
{
	out << mrpt::format(
		"\n----------- [CGridMapAligner::TConfigParams] ------------ \n\n");

	LOADABLEOPTS_DUMP_VAR(methodSelection, int)
	LOADABLEOPTS_DUMP_VAR(featsPerSquareMeter, float)
	LOADABLEOPTS_DUMP_VAR(threshold_max, float)
	LOADABLEOPTS_DUMP_VAR(threshold_delta, float)
	LOADABLEOPTS_DUMP_VAR(min_ICP_goodness, float)
	LOADABLEOPTS_DUMP_VAR(max_ICP_mahadist, double)
	LOADABLEOPTS_DUMP_VAR(maxKLd_for_merge, float)
	LOADABLEOPTS_DUMP_VAR(ransac_minSetSizeRatio, float)
	LOADABLEOPTS_DUMP_VAR(ransac_mahalanobisDistanceThreshold, float)
	LOADABLEOPTS_DUMP_VAR(ransac_chi2_quantile, double)
	LOADABLEOPTS_DUMP_VAR(ransac_prob_good_inliers, double)
	LOADABLEOPTS_DUMP_VAR(ransac_SOG_sigma_m, float)
	LOADABLEOPTS_DUMP_VAR(save_feat_coors, bool)
	LOADABLEOPTS_DUMP_VAR(debug_show_corrs, bool)
	LOADABLEOPTS_DUMP_VAR(debug_save_map_pairs, bool)

	LOADABLEOPTS_DUMP_VAR(feature_descriptor, int)

	feature_detector_options.dumpToTextStream(out);

	out << mrpt::format("\n");
}

/*---------------------------------------------------------------
					loadFromConfigFile
  ---------------------------------------------------------------*/
void CGridMapAligner::TConfigParams::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
	methodSelection =
		iniFile.read_enum(section, "methodSelection", methodSelection);

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
		featsPerSquareMeter, float, iniFile, section)
	MRPT_LOAD_CONFIG_VAR(ransac_SOG_sigma_m, float, iniFile, section)

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_max, float, iniFile, section)
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(threshold_delta, float, iniFile, section)

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(min_ICP_goodness, float, iniFile, section)
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(max_ICP_mahadist, double, iniFile, section)

	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(maxKLd_for_merge, float, iniFile, section)
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
		ransac_minSetSizeRatio, float, iniFile, section)
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
		ransac_mahalanobisDistanceThreshold, float, iniFile, section)
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
		ransac_chi2_quantile, double, iniFile, section)
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(
		ransac_prob_good_inliers, double, iniFile, section)

	MRPT_LOAD_CONFIG_VAR(save_feat_coors, bool, iniFile, section)
	MRPT_LOAD_CONFIG_VAR(debug_show_corrs, bool, iniFile, section)
	MRPT_LOAD_CONFIG_VAR(debug_save_map_pairs, bool, iniFile, section)

	feature_descriptor = iniFile.read_enum(
		section, "feature_descriptor", feature_descriptor, true);
	feature_detector_options.loadFromConfigFile(iniFile, section);
}

CPose3DPDF::Ptr CGridMapAligner::Align3DPDF(
	const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
	const CPose3DPDFGaussian& initialEstimationPDF, float* runningTime,
	void* info)
{
	MRPT_UNUSED_PARAM(m1);
	MRPT_UNUSED_PARAM(m2);
	MRPT_UNUSED_PARAM(initialEstimationPDF);
	MRPT_UNUSED_PARAM(runningTime);
	MRPT_UNUSED_PARAM(info);
	THROW_EXCEPTION("Align3D method not applicable to gridmap-aligner");
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_FREXP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_FREXP_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/config.hpp>
#include <utility>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator() ( A0 a0) const BOOST_NOEXCEPT
    {
      auto that = ifrexp(a0);
      return {that.first, tofloat(that.second)};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::pedantic_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator()(const pedantic_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      auto that = pedantic_(simd::ifrexp)(a0);
      return {that.first, tofloat(that.second)};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::raw_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator()(const raw_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      auto that = raw_(simd::ifrexp)(a0);
      return {that.first, tofloat(that.second)};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0)
                          , bd::cpu_
                          , boost::simd::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator()(const std_tag &, A0 a0 ) const BOOST_NOEXCEPT
    {
      auto that = std_(simd::ifrexp)(a0);
      return {that.first, tofloat(that.second)};
    }
  };
} } }

#endif


// ==========================================================================
// Paradice Dice
//
// Copyright (C) 2013 Matthew Chaplain, All Rights Reserved.
// This file is covered by the MIT Licence:
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
// ==========================================================================
#ifndef PARADICE_DICE_HPP_
#define PARADICE_DICE_HPP_

#include "paradice/export.hpp"
#include "odin/core.hpp"
#include <boost/variant.hpp>
#include <memory>
#include <string>
#include <vector>

namespace paradice {

class beast;
class client;

struct dice_roll
{
    odin::u32 repetitions_;
    odin::u32 amount_;
    odin::u32 sides_;
    odin::s32 bonus_;
};

typedef boost::variant<
    std::weak_ptr<beast>
  , std::weak_ptr<client>
> roller;

struct dice_result
{
    roller    roller_;
    dice_roll roll_;

    // raw dice results per repetition.
    std::vector<std::vector<odin::s32>> results_;
};

PARADICE_EXPORT
std::string describe_dice(dice_roll const &roll);

}

#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CLOSEST_POINTS_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_CLOSEST_POINTS_GEOGRAPHIC_HPP

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/closest_points/services.hpp>
#include <boost/geometry/strategies/geographic/closest_points_pt_seg.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

#include <boost/geometry/strategies/distance/geographic.hpp>

#include <boost/geometry/util/type_traits.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace closest_points
{

template
<
    typename FormulaPolicy = geometry::strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public strategies::distance::geographic<FormulaPolicy, Spheroid, CalculationType>
{
    using base_t = strategies::distance::geographic<FormulaPolicy, Spheroid, CalculationType>;

public:

    geographic() = default;

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    template <typename Geometry1, typename Geometry2>
    auto closest_points(Geometry1 const&, Geometry2 const&,
                        distance::detail::enable_if_ps_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::closest_points::geographic_cross_track
            <
                FormulaPolicy,
                Spheroid,
                CalculationType
            >(base_t::m_spheroid);
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy<Geometry1, Geometry2, geographic_tag, geographic_tag>
{
    using type = strategies::closest_points::geographic<>;
};

} // namespace services

}} // namespace strategies::closest_points

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CLOSEST_POINTS_GEOGRAPHIC_HPP


// Copyright 2019 Victor Hugo Schulz

// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at

//     http://www.apache.org/licenses/LICENSE-2.0

// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <iostream>
#include <fstream>
#include <string>
#include <cmath>
#include <limits>
#include <algorithm>
#include <stdlib.h>
#include <sys/stat.h>

#include <opencv2/core.hpp>
#include <boost/qvm/all.hpp>
#include "structures.h"

using namespace std;
using namespace cv;
using namespace boost::qvm;

// Function declarations:
void load_config(string filename_common, string filename_dut, catalog_parameters &cp, grid_parameters &gp, optical_parameters &op, database_parameters &db);
vector<db> initialize_database(catalog_parameters &cp);
vector<db> visible_stars(vector<db> &database, optical_parameters &op);
void next_neighbor(vector<db> &database, database_parameters &db);
vector<db> reference_stars(vector<db> &vstars);
void calculate_descriptors (vector<db> &vstars, vector<db> &rstars, database_parameters &db, grid_parameters &gp);
double fov(double size, optical_parameters op);
double vangle (vec<double,3> p1, vec<double,3> p2);
quat<double> rot_quat (vec<double,3> v1, vec<double,3> v2);
void proper_motion_correction (hip2 entry, int time_y, db *output);
vector<db> rotate3d(vector<db> visible_stars, vector<db> reference_stars, uint reference_star_id);
double angle_to_origin (db &star);
vector<db> relevant_fov(vector<db> &lf_list, uint reference_star_id, uint reference_nn_id, database_parameters &db);
double angle2d (db &star);
void rotate2d (vector<db> &star_list, double &theta);
void z_norm (vector<db> &star_list, database_parameters &db, grid_parameters &gp);
vector<int> cells (vector<db> &star_list, grid_parameters &gp);
uint bucket_size(int g);
vector<unsigned long> bit_descriptor (vector<int> &cells, grid_parameters &gp);
void sort_nn (vector<db> &stars);
void create_ref(string &ref_db_filename, vector<db> &stars, FileStorage &fs);
void create_index(string &index_filename, vector<db> &stars, FileStorage &fs);
void create_lut(string &lut_db_filename, string &lut_nn_db_filename, vector<db> &stars, grid_parameters &gp, FileStorage &fs);
void create_bin(string &bin_db_filename, vector<db> &stars, grid_parameters &gp, FileStorage &fs);

int main (/*int argc, char **argv*/)
{// Main function.
    catalog_parameters cp; grid_parameters gp; optical_parameters op; database_parameters dp;
    load_config("config/common.yml","config/dut.yml",cp,gp,op,dp);

    vector<db> database = initialize_database(cp);
    vector<db> vstars = visible_stars(database,op);
    next_neighbor(vstars,dp);
    vector<db> rstars = reference_stars(vstars);

    calculate_descriptors(vstars,rstars,dp,gp);
    sort_nn(rstars);

    mkdir("config", S_IRWXU|S_IRWXG|S_IROTH|S_IXOTH);
    mkdir("config/database/", S_IRWXU|S_IRWXG|S_IROTH|S_IXOTH);
    string ref_db_filename =    "config/database/ref_db.bin";
    string lut_db_filename =    "config/database/lut_db.bin";
    string lut_nn_db_filename = "config/database/lut_nn_db.bin";
    string bin_db_filename =    "config/database/bin_db.bin";
    string index_filename =     "config/database/index.bin";

    FileStorage fs("config/database.yml", FileStorage::WRITE);

    fs << "ref_db_filename" << ref_db_filename;
    create_ref(ref_db_filename,rstars,fs);

    fs << "lut_db_filename" << lut_db_filename;
    fs << "lut_nn_db_filename" << lut_nn_db_filename;
    create_lut(lut_db_filename,lut_nn_db_filename,rstars,gp,fs);

    fs << "bin_db_filename" << bin_db_filename;
    create_bin(bin_db_filename,rstars,gp,fs);

    fs << "index_filename" << index_filename;
    create_index(index_filename,rstars,fs);
    fs.release();
    return 0;
}

void load_config(string filename_common, string filename_dut, catalog_parameters &cp, grid_parameters &gp, optical_parameters &op, database_parameters &dp)
{// Load configuration from file.
    double dtmp;
    Mat mtmp;
    FileStorage fs(filename_common, FileStorage::READ);
    fs["cp_hipparcos_filename"]  >> cp.hipparcos;
    fs["cp_hipparcos2_filename"] >> cp.hipparcos2;
    fs["cp_hipparcos_size"]      >> dtmp; cp.hipparcos_size = (int)dtmp;
    fs["cp_epoch"]               >> dtmp; cp.epoch = (int)dtmp;
    fs["op_resolution"]          >> mtmp; op.resolution = Point2i(mtmp);
    fs["op_pixel_size"]          >> mtmp; op.pixel_size = Point2d(mtmp);
    fs["op_sensor_size"]         >> mtmp; op.sensor_size = Point2d(mtmp);
    fs["op_focus"]               >> op.focus;
    fs["op_center"]              >> mtmp; op.center = Point2i(mtmp);
    fs["op_max_mag"]             >> op.max_mag;
    fs.release();
    FileStorage fs2(filename_dut, FileStorage::READ);
    fs2["gp_g"]                     >> dtmp; gp.g = (int)dtmp;
    fs2["gp_pattern_radius"]        >> dtmp; gp.pattern_radius = (int)dtmp;
    fs2["gp_buffer_radius"]         >> gp.buffer_radius;
    fs2["gp_confidence_factor"]     >> dtmp; gp.confidence_factor = (int)dtmp;
    fs2["gp_expercted_false_stars"] >> dtmp; gp.expected_false_stars = (int)dtmp;
    fs2["gp_minimim_match"]         >> dtmp; gp.minimum_match = (int)dtmp;
    fs2["gp_nn_error_angle"]        >> gp.error;
    fs2.release();
    dp.pattern_radius   = fov(gp.pattern_radius*op.pixel_size.y, op);
    dp.buffer_radius    = fov(gp.buffer_radius*op.pixel_size.y, op);
    dp.min_dist         = fov(norm(op.pixel_size), op);
    dp.relevance_radius = fov((norm(op.sensor_size)),op);
}

vector<db> initialize_database(catalog_parameters &cp)
{// Read information from Hipparcos and Hipparcos 2 catalogs for initializing the database.
    hip *hipparcos = new hip [cp.hipparcos_size];
    hip initial = {0,'\0',0};
    for(int i=0;i<cp.hipparcos_size;i++)
        hipparcos[i] = initial;

    double time_y = cp.epoch - 1991.25;

    // Hipparcos
    string line;
    ifstream myfile (cp.hipparcos);
    if (myfile.is_open()) {
        while ( getline (myfile,line) ) {
            hip data;
            string sstr;
            int HIP      = stoi(line.substr(8,6));
            data.Proxy   = line.substr(15,1).compare(" ") ? 1:0;
            sstr = line.substr(41,5);
            if (!sstr.compare("     ")){
	        // cerr << "Warning: Empty Vmag (HIP=" << HIP << ")" << endl;
                continue;
            }
            data.Vmag    = stod(sstr);
            sstr = line.substr(47,1);
            if (sstr.compare(" "))
                data.VarFlag = stoi(sstr);
            else
                data.VarFlag = 0;
            hipparcos[HIP] = data;
        }
        myfile.close();
    }
    else {
        cout << "Unable to open file \"" << cp.hipparcos2 << "\"." << endl;
        exit(1);
    }

    // Hipparcos 2
    vector<hip2> hipparcos2;
    ifstream myfile2 (cp.hipparcos2);
    if (myfile2.is_open()) {
        while ( getline (myfile2,line) ) {
            hip2 data;
            data.HIP   = stoi(line.substr(0,6));
            data.RArad = stod(line.substr(15,13));
            data.DErad = stod(line.substr(29,13));
            data.pmRA  = stod(line.substr(51,8));
            data.pmDE  = stod(line.substr(60,8));
            hipparcos2.push_back(data);
        }
        myfile2.close();
    }
    else {
        cout << "Unable to open file \"" << cp.hipparcos2 << "\"." << endl;
        exit(1);
    }

    // Database
    vector<db> database;
    for(uint i=0;i<hipparcos2.size();i++){
        db entry;
        entry.id         = hipparcos2[i].HIP;
        entry.mag        = hipparcos[entry.id].Vmag;
        entry.proxy_flag = hipparcos[entry.id].Proxy;
        entry.var_flag   = hipparcos[entry.id].VarFlag;
        proper_motion_correction(hipparcos2[i],time_y,&entry);
        entry.nn = -1;
        entry.angle_nn = DBL_MAX;
        database.push_back(entry);
    }
    delete [] hipparcos;
    return database;
}

vector<db> visible_stars(vector<db> &database, optical_parameters &op)
{// Extract only the visible stars from the database.
    vector<db> output;
    for(uint i=0;i<database.size();i++){
        if(database[i].mag <= op.max_mag)
            output.push_back(database[i]);
    }
    return output;
}

void next_neighbor(vector<db> &database, database_parameters &db)
{// Calculate the next neighbor pointer and distance, populating the database entries.
    for(uint i=0;i<database.size()-1;i++){
        for(uint j=i+1;j<database.size();j++){
            double angle = vangle(database[i].uv,database[j].uv);
            if(angle < db.buffer_radius){
                database[i].proxy_flag = 1;
                database[j].proxy_flag = 1;
            }
            else{
                if(angle < database[i].angle_nn){
                    database[i].nn = j;
                    database[i].angle_nn = angle;
                }
                if(angle < database[j].angle_nn){
                    database[j].nn = i;
                    database[j].angle_nn = angle;
                }
            }
        }
    }
}

vector<db> reference_stars(vector<db> &vstars)
{// Remove variable stars and stars with very close neighbors (double, triple...).
    vector<db> output;
    for(uint i=0;i<vstars.size();i++){
        if (!((vstars[i].var_flag >= 3) || vstars[i].proxy_flag))
            output.push_back(vstars[i]);
    }
    return output;
}

void calculate_descriptors (vector<db> &vstars, vector<db> &rstars, database_parameters &dp, grid_parameters &gp)
{// Populate the descriptor entries of the reference stars in the database.
    for(uint ref_star_id=0; ref_star_id<rstars.size(); ref_star_id++){
        int ref_nn_id = rstars[ref_star_id].nn;
        vector<db> rotated = rotate3d(vstars,rstars,ref_star_id);
        vector<db> cropped = relevant_fov(rotated,ref_star_id,ref_nn_id,dp);
        double theta = angle2d(rotated[rstars[ref_star_id].nn]);
        rotate2d(cropped,theta);
        z_norm(cropped,dp,gp);
        rstars[ref_star_id].angle_2d = theta;
        rstars[ref_star_id].cells_descriptor = cells(cropped,gp);
        rstars[ref_star_id].bit_descriptor = bit_descriptor(rstars[ref_star_id].cells_descriptor,gp);
    }
}

double vangle (vec<double,3> p1, vec<double,3> p2)
{// Angle between two vectors.
    return atan2(mag(cross(p1,p2)), dot(p1,p2));
}

quat<double> rot_quat (vec<double,3> v1, vec<double,3> v2)
{// Rotation quaternion between two unit vectors. V1 must be different than V2.
    double theta = vangle(v1,v2);
    double angle = cos(theta/2);
    vec<double,3> v = cross(v1,v2);
    normalize(v);
    v *= sin(theta/2);
    quat<double> output;
    output = {angle,v.a[0],v.a[1],v.a[2]};
    return output;
}

double fov(double size, optical_parameters op)
{// Field of view calculation.
    return 2*atan2(size,2*op.focus);
}

void proper_motion_correction(hip2 entry, int time_y, db *output)
{// Proper motion conversion, with output as an unit vector.
    double mu_alpha_rad = (entry.pmRA * M_PI) / (3600 * 1000 * 180 * cos(entry.DErad));
    double mu_delta_rad = (entry.pmDE * M_PI) / (3600 * 1000 * 180);
    double alpha        = entry.RArad + mu_alpha_rad * time_y;
    double delta        = entry.DErad + mu_delta_rad * time_y;
    output->uv = {cos(alpha) * cos(delta),
                  sin(alpha) * cos(delta),
                  sin(delta)};
}

vector<db> rotate3d(vector<db> visible_stars, vector<db> reference_stars, uint reference_star_id)
{// Rotate the database so that the reference star will appear on the origin.
    vec<double,3> origin = {0,0,1};
    quat<double> q = rot_quat(reference_stars[reference_star_id].uv, origin);
    for(uint i = 0; i<visible_stars.size(); i++){
        vec<double,3> p_ = visible_stars[i].uv;
        quat<double> p = {0,p_.a[0],p_.a[1],p_.a[2]};
        quat<double> rot_uv = q * p * inverse(q);
        visible_stars[i].uv = V(rot_uv);
    }
    return visible_stars;
}

double angle_to_origin (db &star)
{// Angle between the star in local frame and the coordinate system origin.
    vec<double,3> origin = {0,0,1};
    return vangle(star.uv, origin);
}

vector<db> relevant_fov(vector<db> &lf_list, uint reference_star_id, uint reference_nn_id, database_parameters &dp)
{// Remove stars which are outside of the field of view.
    vector<db> fov;
    for(uint i = 0; i<lf_list.size(); i++){
        if((i != reference_star_id)
                && (i != reference_nn_id)
                && (angle_to_origin(lf_list[i]) < dp.relevance_radius))
        { // Reference star and its next neighbor are excluded.
            fov.push_back(lf_list[i]);
        }
    }
    return fov;
}

double angle2d (db &star)
{// Returns the angle needed to rotate a given star so that it will fall in the x axis.
    return -atan2(star.uv.a[1], star.uv.a[0]);
}

void rotate2d (vector<db> &star_list, double &theta)
{// In-place rotation of a list of stars by a given angle.
    quat<double> q = rotz_quat(theta);
    for(uint i=0; i<star_list.size();i++){
        quat<double> p = {0,
                          star_list[i].uv.a[0],
                          star_list[i].uv.a[1],
                          star_list[i].uv.a[2]};
        star_list[i].uv = V(q*p*inverse(q));
    }
}

void z_norm (vector<db> &star_list, database_parameters &dp, grid_parameters &gp)
{// In-place linear transformation so that points fall within x=[0,g] and y=[0,g].
 // Since pr =! rr, out of bound values must be filtered later.
 // The z coordinate remains unchanged.
    for(uint i=0; i<star_list.size(); i++){
        double norm = star_list[i].uv.a[2] * tan(dp.pattern_radius) * 2;
        star_list[i].uv.a[0] /= norm;
        star_list[i].uv.a[1] /= norm;
        star_list[i].uv.a[0] += 0.5;
        star_list[i].uv.a[1] += 0.5;
        star_list[i].uv.a[0] *= gp.g;
        star_list[i].uv.a[1] *= gp.g;
        star_list[i].uv.a[0] = floor(star_list[i].uv.a[0]);
        star_list[i].uv.a[1] = floor(star_list[i].uv.a[1]);
    }
}

vector<int> cells (vector<db> &star_list, grid_parameters &gp)
{// Returns a list with the cells present on the descriptor.
    vector<int> cells;
    for(uint i=0; i<star_list.size(); i++){
        if((star_list[i].uv.a[0] >= 0) &&
                (star_list[i].uv.a[0] < gp.g) &&
                (star_list[i].uv.a[1] >= 0) &&
                (star_list[i].uv.a[1] < gp.g)){
            cells.push_back(gp.g*star_list[i].uv.a[1] + star_list[i].uv.a[0]);
        }
    }
    // Duplicates are removed (next 2 lines).
    sort(cells.begin(),cells.end());
    cells.erase(unique(cells.begin(),cells.end()), cells.end());
    return cells;
}

uint bucket_size(int g)
{// Calculates the number of buckets for the descriptor calculation from the grid size g; ex. ceil(g*g/64) for 64 bits.
    int g2 = g*g;
    int divisor = sizeof(long) << 3;
    return g2/divisor + (g2%divisor ? 1 : 0);
}

vector<unsigned long> bit_descriptor (vector<int> &cells, grid_parameters &gp)
{// Calculates a bit descriptor from the on cells.
    static unsigned int bit_size = sizeof(long) * 8; // Architecture dependent result
    // Since the needed bits usually exceed the architecture word lenghts, multiple 'buckets' hold the values.
    uint bsize = bucket_size(gp.g);
    unsigned long bucket[bsize];
    unsigned int i;
    for(i=0;i<bsize;i++)
        bucket[i] = 0;
    for(i=0;i<cells.size();i++){
        unsigned int n = cells[i]/bit_size;
        bucket[n] |= (unsigned long)1<<(cells[i]%bit_size);
    }
    vector<unsigned long> output(bucket, bucket + sizeof bucket / sizeof bucket[0]);
    return output;
}

void sort_nn (vector<db> &stars)
{// Sorts the database by the next neighbor angle.
    qsort(stars.data(),stars.size(),sizeof(db),[](const void *a_, const void *b_){
        db *a = (db*) a_;
        db *b = (db*) b_;
        return (int)(a->angle_nn > b->angle_nn);
    });
}

void create_ref(string &ref_db_filename, vector<db> &stars, FileStorage &fs)
{// Produces the reference database and writes it to a file. Format: {id, angle_nn, uv_x, uv_y, uv_z}
    int rows = stars.size();
    int cols = 5;
    double ref_db[rows][cols];
    for(uint i=0;i<stars.size();i++){
        ref_db[i][0] = stars[i].id;
        ref_db[i][1] = stars[i].angle_nn;
        ref_db[i][2] = stars[i].uv.a[0];
        ref_db[i][3] = stars[i].uv.a[1];
        ref_db[i][4] = stars[i].uv.a[2];
    }
    fs << "ref_db_rows" << rows;
    fs << "ref_db_cols" << cols;
    FILE *fp;
    fp = fopen(ref_db_filename.data(), "w");
    fwrite(ref_db,1,sizeof(ref_db),fp);
    fclose(fp);
}

int binSearch2(double what, double *data, double len)
{// Binary search method.
    int low = 0;
    int high = len - 1;
    int mid = 0;
    while (low <= high) {
        mid = (low + high) / 2;
        if (data[mid] > what)
            high = mid - 1;
        else if (data[mid] < what)
            low = mid + 1;
        else
            return mid;
    }
    return mid;
}

void create_index(string &index_filename, vector<db> &stars, FileStorage &fs)
{// Creates an index to speed up the lookup of elements by their nn distance.
    double last = stars[stars.size()-1].angle_nn;
    double data[stars.size()];
    for(uint i=0; i<stars.size();i++){
        data[i] = stars[i].angle_nn;
    }
    int multiplier = 1;
    while(true){
        multiplier *= 2;
        last *= 2;
        if(last > stars.size()) break;
    }
    int real_size = (int)(last+1);
    short index[real_size];
    for(int i=0; i<real_size; i++){
        index[i] = (short)binSearch2((double)i/multiplier,data,stars.size());
    }
    fs << "index_size" << real_size;
    fs << "index_multiplier" << multiplier;
    FILE *fp;
    fp = fopen(index_filename.data(), "w");
    fwrite(index,sizeof(short),sizeof(index),fp);
    fclose(fp);
}

void create_lut(string &lut_db_filename, string &lut_nn_db_filename, vector<db> &stars, grid_parameters &gp, FileStorage &fs)
{// Produces the (inverted) lookup table and writes it to a file.
    uint rows = (uint)gp.g*gp.g;
    vector<vector <short> > lut(rows, vector<short>());
    vector<vector <float> > lut_nn(rows, vector<float>());
    for(uint i=0;i<stars.size();i++){
        for(uint j=0;j<stars[i].cells_descriptor.size();j++){
            lut[ stars[i].cells_descriptor[j] ].push_back((short)i);
            lut_nn[ stars[i].cells_descriptor[j] ].push_back((float)stars[i].angle_nn);
        }
    }
    uint cols = 0;
    for(uint i=0;i<lut.size();i++){
        if(lut[i].size() > cols){
            cols = lut[i].size();
        }
    }
//    short lut_array[rows][cols];
//    for(uint i=0;i<rows;i++){
//        for(uint j=0;j<cols;j++){
//            lut_array[i][j] = -1;
//        }
//    }
    short * lut_array = new short[rows*cols];
    for(uint i=0; i<rows*cols;i++){
        lut_array[i] = -1;
    }
    float *lut_array_nn = new float[rows*cols];
    for(uint i=0; i<rows*cols;i++){
        lut_array_nn[i] = DBL_MAX;
    }
    for(uint i=0;i<rows;i++){
        //memcpy(&lut_array[i],lut[i].data(),lut[i].size()*sizeof(short));
        memcpy(&lut_array[i*cols],lut[i].data(),lut[i].size()*sizeof(short));
        memcpy(&lut_array_nn[i*cols],lut_nn[i].data(),lut_nn[i].size()*sizeof(float));
    }
    fs << "lut_db_rows" << (int)rows;
    fs << "lut_db_cols" << (int)cols;
    FILE *fp;
    fp = fopen(lut_db_filename.data(), "w");
//    fwrite(lut_array,sizeof(short),sizeof(lut_array),fp);
    fwrite(lut_array,sizeof(short),rows*cols,fp);
    fclose(fp);
    fp = fopen(lut_nn_db_filename.data(), "w");
    fwrite(lut_array_nn,sizeof(float),rows*cols,fp);
    fclose(fp);
    delete lut_array_nn;
}

void create_bin(string &bin_db_filename, vector<db> &stars, grid_parameters &gp, FileStorage &fs)
{// Produces the bit descriptor and writes it to a file.
    uint rows = stars.size();
    uint cols = bucket_size(gp.g);
    unsigned long bin_db[rows][cols];
    for(uint i=0;i<rows;i++){
        memcpy(&bin_db[i],stars[i].bit_descriptor.data(),cols*sizeof(unsigned long));
    }
    fs << "bin_db_rows" << (int)rows;
    fs << "bin_db_cols" << (int)cols;
    FILE *fp;
    fp = fopen(bin_db_filename.data(), "w");
    fwrite(bin_db,1,sizeof(bin_db),fp);
    fclose(fp);
}


// this must define the same symbol as the main module file (numpy requirement)
#define PY_ARRAY_UNIQUE_SYMBOL andres_graph_PyArray_API
#define NO_IMPORT_ARRAY

#include <Python.h>
#include <boost/python.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include <vigra/multi_array.hxx>
#include <vigra/tinyvector.hxx>

#include "andres/graph/graph.hxx"
#include "andres/graph/grid-graph.hxx"

#include "graph_exporter.hxx"

namespace bp = boost::python;
namespace agraph = andres::graph;

void insertGridGraphEdges2D(agraph::Graph< > & self, const agraph::GridGraph<2> & gg){
    for(std::size_t e=0; e<gg.numberOfEdges(); ++e){
        auto v0 = gg.vertexOfEdge(e, 0);
        auto v1 = gg.vertexOfEdge(e, 1);
        self.insertEdge(v0, v1);
    }
}
template<class T>
void insertEdges(agraph::Graph< > & self, const vigra::NumpyArray<1, vigra::TinyVector<T, 2> >  & edges){
    for(const auto & edge : edges){
        self.insertEdge(edge[0],edge[1]);
    }
}

void exportGraph(){
    // initialize numpy and vigranumpy
   bp::class_<agraph::Graph< > > ("Graph",bp::init<const size_t >(bp::arg("numberOfNodes")=0))
        .def(GraphExporter<agraph::Graph<> >("Graph"))
        .def("insertGridGraphEdges",&insertGridGraphEdges2D)
        .def("insertEdges",vigra::registerConverters(&insertEdges<int64_t>))
        .def("insertEdges",vigra::registerConverters(&insertEdges<uint64_t>))
        .def("insertEdges",vigra::registerConverters(&insertEdges<int32_t>))
        .def("insertEdges",vigra::registerConverters(&insertEdges<uint32_t>))
   ;
}


//
// Created by michel on 10-9-16.
//

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

/**
 * This serves just to keep the clang analyser happy
 */
BOOST_AUTO_TEST_CASE(placeHolder) { BOOST_CHECK(true); }

//#define TDAP_FOLLOWERS_DEBUG_LOGGING 3

//#include <atomic>
//#include <cstdio>
//#include <iostream>
//#include <jack/jack.h>
//#include <random>
//#include <tdap/Crossovers.hpp>
//#include <tdap/Delay.hpp>
//#include <tdap/IirBiquad.hpp>
//#include <tdap/PeakDetection.hpp>
//#include <tdap/PerceptiveRms.hpp>
//#include <tdap/TrueFloatingPointWindowAverage.hpp>
//
//#include "TestAlignedFrame.hpp"
//#include "TestIirCoefficients.hpp"
//#include "TestVolumeMatrix.hpp"
//#include "TestConfig.hh"
//
//using namespace tdap;
//using namespace std;
//
//static void testTrueAverage() {
//  const size_t maxWindowSize = 100;
//  const size_t errorTimeConstant = maxWindowSize * 100;
//  const double relativeErrorNoise = 1e-6;
//  const double amplitude = 1.0;
//  const size_t largeWindow = 100;
//  const size_t smallWindow = 10;
//  const size_t printInterval = 1;
//
//  TrueFloatingPointWeightedMovingAverage<double> smallAverage(
//      maxWindowSize, errorTimeConstant);
//  TrueFloatingPointWeightedMovingAverage<double> largeAverage(
//      maxWindowSize, errorTimeConstant);
//  TrueFloatingPointWeightedMovingAverageSet<double> set(
//      maxWindowSize, errorTimeConstant, 2, 0.0);
//
//  smallAverage.setAverage(0);
//  largeAverage.setAverage(0);
//  set.setAverages(0);
//
//  printf("\nSetting small average:\n");
//  smallAverage.setWindowSize(smallWindow);
//  printf("\nSetting large average:\n");
//  largeAverage.setWindowSize(largeWindow);
//  printf("\nSetting set, small average:\n");
//  set.setWindowSizeAndScale(0, smallWindow, 1.0);
//  printf("\nSetting set, large average:\n");
//  set.setWindowSizeAndScale(1, largeWindow, 2.0);
//
//  printf("Start....\n");
//  for (size_t i = 0; i < largeWindow * 5; i++) {
//    const double input =
//        i > largeWindow && i <= 2 * largeWindow ? amplitude : 0.0;
//    smallAverage.addInput(input);
//    largeAverage.addInput(input);
//    double setAvg = set.addInputGetMax(input, 0.0);
//    if (i % printInterval == 0) {
//      printf("[%5zu] input=%8.3lf ; avg1=%18.16lf ; avg2=%18.16lf ; "
//             "set1=%18.16lf ; set2=%18.16lf ; setMax=%18.16lf\n",
//             i, input, smallAverage.getAverage(), largeAverage.getAverage(),
//             set.getAverage(0), set.getAverage(1), setAvg);
//    }
//  }
//}
//
//static constexpr size_t RANGE = 100;
//static constexpr size_t THRESHOLD = 25;
//static constexpr size_t WINDOW = 40;
//static constexpr size_t PRINT_INTERVAL = 1;
//static constexpr size_t INTERVAL = Value<size_t>::min(PRINT_INTERVAL, WINDOW);
//static constexpr size_t RUNLENGTH = WINDOW * 10;
//static constexpr size_t PERIODS = 5;
//static constexpr size_t RANDOM_PEAK = 73;
//
//static const double *getInput() {
//  static double *input = new double[RUNLENGTH];
//  static std::atomic_flag generated;
//
//  if (!generated.test_and_set()) {
//    for (size_t i = 0; i < RUNLENGTH; i++) {
//      input[i] =
//          THRESHOLD * (1.0 + sin(M_2_PI * i / PERIODS) * rand() / RAND_MAX);
//      if (i % RANDOM_PEAK == 0) {
//        input[i] += 50;
//      }
//    }
//  }
//
//  return input;
//}
//
//static const double *getSpikedInput() {
//  static double *input = new double[RUNLENGTH];
//  static std::atomic_flag generated;
//
//  if (!generated.test_and_set()) {
//    bool flip = false;
//    int peak1 = -1;
//    int peak2 = -1;
//    size_t i;
//    for (i = 0; i < 8 * RUNLENGTH / 10; i++) {
//      if (i % RANDOM_PEAK == 0) {
//        flip = !flip;
//        input[i] = 75;
//        peak1 = RANDOM_PEAK / 10;
//      } else if (peak1 == 0) {
//        input[i] = flip ? 80 : 70;
//        peak2 = RANDOM_PEAK / 10;
//      } else if (peak2 == 0) {
//        input[i] = flip ? 70 : 80;
//      } else {
//        input[i] = THRESHOLD * (1.0 + sin(M_2_PI * i / PERIODS));
//      }
//      peak1--;
//      peak2--;
//    }
//    for (; i < RUNLENGTH; i++) {
//      input[i] = 0;
//    }
//  }
//  return input;
//}
//
//template <typename T, bool IS_NUMBER> struct Zero;
//
//template <typename T> struct Zero<T, false> {
//  static const T zero() {
//    T z;
//    return z;
//  }
//};
//
//template <typename T> struct Zero<T, true> {
//  static const T zero() { return 0; }
//};
//
//template <typename T> class Scenario {
//  using Zeroer = Zero<T, is_floating_point<T>::value || is_integral<T>::value>;
//
//  T *data_ = nullptr;
//  size_t capacity_ = 0;
//  size_t size_ = 0;
//
//  void ensureCapacity(size_t capacity) {
//    if (capacity < capacity_) {
//      return;
//    }
//    size_t newCapacity;
//    if (capacity_ == 0) {
//      newCapacity = Sizes::min(10, capacity + 1);
//    } else {
//      newCapacity = Sizes::max(capacity_ * 3 / 2, capacity + 1);
//    }
//    T *newData = new T[newCapacity];
//    size_t i;
//    for (i = 0; i < capacity_; i++) {
//      newData[i] = data_[i];
//    }
//    for (; i < newCapacity; i++) {
//      newData[i] = Zeroer::zero();
//    }
//    if (data_) {
//      delete[] data_;
//    }
//    data_ = newData;
//    capacity_ = newCapacity;
//  }
//
//  void ensureCapacityForIndex(size_t index) { ensureCapacity(index + 1); }
//
//public:
//  const size_t size() const { return size_; }
//
//  T &operator[](size_t index) {
//    ensureCapacityForIndex(index);
//    if (index >= size_) {
//      size_ = index + 1;
//    }
//    return data_[index];
//  }
//
//  const T operator()(size_t index) const {
//    return data_[IndexPolicy::force(index, size_)];
//  }
//
//  void clear() { size_ = 0; }
//
//  void enlarge(size_t with) {
//    ensureCapacity(size_ + with);
//    size_ += with;
//  }
//
//  void foreach (void (*action)(const T)) {
//    for (size_t i = 0; i < size(); i++) {
//      action(operator()(i));
//    }
//  }
//
//  ~Scenario() {
//    if (data_) {
//      delete[] data_;
//    }
//    capacity_ = 0;
//    size_ = 0;
//  }
//};
//
//struct Measurement {
//  ssize_t t = 0;
//  double in = 0;
//  double out1 = 0;
//  double out2 = 0;
//
//  void print() const { printf("%zi   %lf   %lf   %lf\n", t, in, out1, out2); }
//};
//
//static void printMeasurement(const Measurement m) { m.print(); }
//
//static void testTriangularFollowerSmoothed() {
//  //    TriangularFollower<double> fast;
//
//  static constexpr size_t ATTACK = 4;
//  static constexpr size_t RELEASE = 8;
//  static constexpr size_t SIZE = 1000;
//  static constexpr double THRESHOLD = 100;
//  static constexpr double SMOOTHING = 10;
//  static constexpr double INTEGRATION = ATTACK / SMOOTHING;
//  static constexpr size_t TOTAL_DELAY = ATTACK + INTEGRATION;
//
//  TriangularFollower<double> follower1(SIZE);
//  IntegrationCoefficients<double> smooth(INTEGRATION);
//
//  follower1.setTimeConstantAndSamples(ATTACK, RELEASE, THRESHOLD);
//  Scenario<double> input;
//
//  size_t scenario;
//
//  scenario = 65;
//  input[scenario] = 200;
//  input[scenario + 3] = 300;
//
//  scenario = 25;
//  input[scenario] = 200;
//  input[scenario + 3] = 185;
//  input[scenario + 5] = 120;
//
//  scenario = 45;
//  input[scenario] = 200;
//  input[scenario + 2] = 185;
//  input[scenario + 4] = 175;
//
//  scenario = 5;
//  input[scenario] = 200;
//  input[scenario + 1] = 204;
//  input[scenario + 2] = 202;
//  input[scenario + 3] = 202;
//  input[scenario + 4] = 202;
//  input[scenario + 5] = 202;
//  input[scenario + 6] = 202;
//  input[scenario + 7] = 201;
//
//  //    for (size_t i = 0; i < SIZE; i++) {
//  //        input[i] = 0.5 * THRESHOLD + THRESHOLD * rand() / RAND_MAX;
//  //    }
//  input.enlarge(ATTACK + RELEASE);
//
//  printf("\n# Triangular follower ***\n\n");
//
//  Scenario<Measurement> output;
//
//  double int1 = THRESHOLD;
//  double max = THRESHOLD;
//
//  for (size_t i = 0; i < input.size(); i++) {
//    //        double out1 = follower2.follow(inputShort(i));
//    //        double out2 = follower1.follow(input(i));
//    double out1 = follower1.follow(input(i));
//    smooth.integrate(out1, int1);
//    double in = i < TOTAL_DELAY ? 0 : input(i - ATTACK);
//    ssize_t time = i;
//    time -= ATTACK;
//    output[i] = {time, in, out1, int1}; // in * THRESHOLD / int1};
//  }
//
//  output.foreach (printMeasurement);
//}
//
//static void reachIngForFactors() {
//  static constexpr size_t RC = 8;
//  static constexpr size_t MUL = 20;
//  static constexpr size_t COUNT = RC * MUL;
//
//  IntegrationCoefficients<double> integration(RC);
//  double mem1 = 0.0;
//  double mem2 = 0.0;
//  double mem3 = 0.0;
//  double mem4 = 0.0;
//
//  for (size_t i = 1; i <= COUNT; i++) {
//    double input = (-1.0 + i) / COUNT;
//    integration.integrate(input, mem1);
//    integration.integrate(mem1, mem2);
//    integration.integrate(mem2, mem3);
//    integration.integrate(mem3, mem4);
//    printf("# \tin=%lf, mem1=%lf, mem2=%lf\n", input, mem1, mem2);
//  }
//
//  printf("# Double integration of linear [0..1] slope with %zu times "
//         "integration time yields %lf/%lf\n",
//         MUL, mem2, 1 / mem2);
//}
//
//static void integrationTimes() {
//  static constexpr size_t RC = 16;
//  static constexpr size_t MUL = 6;
//  static constexpr size_t COUNT = RC * MUL;
//
//  IntegrationCoefficients<double> integration(RC);
//  double mem1 = 0.0;
//  double mem2 = 0.0;
//  double mem3 = 0.0;
//  double mem4 = 0.0;
//
//  for (size_t i = 1; i <= COUNT; i++) {
//    integration.integrate(1.0, mem1);
//    integration.integrate(mem1, mem2);
//    integration.integrate(mem2, mem3);
//    integration.integrate(mem3, mem4);
//    printf("# \tint-1=%lf, int-2=%lf, int-3=%lf, int-4=%lf\n", mem1, mem2, mem3, mem4);
//  }
//}
//
//static void testPeakDetector() {
//
//  PeakMemory<double> memory(288);
//  PeakDetector<double> detector(288, 0.5, 0.3, 1);
//
//  char line[RANGE + 1];
//
//  const double *input = getSpikedInput();
//  size_t samples = WINDOW;
//  memory.setSampleCount(WINDOW);
//  static constexpr const char *DIGIT = " iP*";
//
//  //    for (size_t i = 0; i < RUNLENGTH; i++) {
//  //        double in = input[i];
//  //        double peak = memory.addSampleGetPeak(in);
//  //        double delayed = (i >= samples) ? input[i - samples] : 0.0;
//  //        double fault = delayed / peak;
//  //        if (fault > 1 || (i % INTERVAL == 0)) {
//  //            for (size_t at = 0; at < RANGE; at++) {
//  //                int dig = 0;
//  //                if (at == static_cast<size_t>(delayed)) {
//  //                    dig |= 1;
//  //                }
//  //                if (at == static_cast<size_t>(peak)) {
//  //                    dig |= 2;
//  //                }
//  //                line[at] = DIGIT[dig];
//  //            }
//  //            line[RANGE] = '\0';
//  //            if (fault > 0) {
//  //                printf("[%5zu]\t[%s] %6.04lf\n", i, line, fault);
//  //            }
//  //            else {
//  //                printf("[%5zu]\t[%s] %7s\n", i, line, "");
//  //            }
//  //        }
//  //    }
//
//  memory.setSampleCount(WINDOW);
//  samples = detector.setSamplesAndThreshold(WINDOW, THRESHOLD);
//
//  printf("\n# Using LIMITER\n\n");
//  printf("Sample Input Detect Output\n");
//  static constexpr const char *DIGIT_LIMIT = " io*dDdD";
//  double maxFault = 0;
//  for (size_t i = 0; i < RUNLENGTH; i++) {
//    double detect = detector.addSampleGetDetection(input[i]);
//    double in = i >= samples ? input[i - samples] : 0.0;
//    double gain = static_cast<double>(THRESHOLD) / detect;
//    double out = in * gain;
//    double fault = in / detect;
//    printf("%5zu %6.02lf %6.02lf\n", i, in, detect);
//  }
//  printf("Maximum fault: %lf\n", maxFault);
//}
//
//static void
//printDelayEntry(const typename MultiChannelAndTimeDelay<int>::Entry &entry) {
//  cout << "\tdelay=" << entry.delay_ << "; end=" << entry.end_
//       << "; write=" << entry.write_ << "; read=" << entry.read_ << endl;
//}
//
//static void printDelayState(const MultiChannelAndTimeDelay<int> &delay) {
//  cout << "Delay channels=" << delay.channels_
//       << "; max-channels=" << delay.maxChannels_ << endl;
//  for (size_t i = 0, t = 0; t <= delay.maxDelay_; t++) {
//    cout << "\t[" << t << "]";
//    for (size_t channel = 0; channel < delay.channels_; channel++, i++) {
//      cout << " " << delay.buffer_[i];
//    }
//    cout << endl;
//  }
//  for (size_t channel = 0; channel < delay.channels_; channel++) {
//    printDelayEntry(delay.entry_[channel]);
//  }
//};
//
//void testMultiTimeDelay() {
//  MultiChannelAndTimeDelay<int> delay(4, 4);
//  delay.setChannels(4);
//  printDelayState(delay);
//  delay.setDelay(0, 0);
//  printDelayState(delay);
//  delay.setDelay(1, 1);
//  printDelayState(delay);
//  delay.setDelay(2, 2);
//  printDelayState(delay);
//  delay.setDelay(3, 3);
//
//  for (int i = 0; i < 100; i++) {
//    cout << "channel 0: getAndSet(" << (i + 100)
//         << ") = " << delay.setAndGet(0, i + 100) << endl;
//    cout << "channel 1: getAndSet(" << (i + 200)
//         << ") = " << delay.setAndGet(1, i + 200) << endl;
//    cout << "channel 2: getAndSet(" << (i + 300)
//         << ") = " << delay.setAndGet(2, i + 300) << endl;
//    cout << "channel 3: getAndSet(" << (i + 400)
//         << ") = " << delay.setAndGet(3, i + 400) << endl;
//    printDelayState(delay);
//    delay.next();
//  }
//}
//
//void printPinkNoise() {
//  constexpr size_t crossoverCount = 2;
//  tdap::FixedSizeArray<double, crossoverCount> crossovers;
//  crossovers[0] = 80;
//  crossovers[1] = 120;
//  tdap::FixedSizeArray<double, 2 + 2 *crossoverCount> weights =
//      Crossovers::weights(crossovers, 96000);
//  for (size_t i = 0; i < weights.capacity(); i++) {
//    std::cout << "w[" << i << "]=" << weights[i] << std::endl;
//  }
//}
//
//int main(int c, const char *args[]) {
//  testConfig();
//  testVolumeMatrix();
//  testGroupVolumeMatrix();
//  integrationTimes();
//  //  alignedFrameTests();
//  //  testIirCoefficientVariants();
//  //	testMultiTimeDelay();
//  //	testTrueAverage();
//  //        testTriangularFollowerSmoothed();
//  //        testPeakDetector();
//  //    std::cout << tdap::Perceptive::Metrics::createWithEvenSteps(
//  //            0.6, 0.001, 15) << std::endl;
//  //        printPinkNoise();
//
//  //        reachIngForFactors();
//  return 0;
//}
//
//