// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/functional/hash.hpp>
#include <cstddef>
#include <optional>
#include <ostream>
#include <pup.h>
#include <tuple>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataBox/DataBox.hpp"
#include "DataStructures/Tensor/EagerMath/Magnitude.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Domain.hpp"
#include "Domain/FaceNormal.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/DirectionMap.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Tags.hpp"
#include "Domain/Tags/FaceNormal.hpp"
#include "Domain/Tags/Faces.hpp"
#include "Domain/Tags/SurfaceJacobian.hpp"
#include "Elliptic/BoundaryConditions/ApplyBoundaryCondition.hpp"
#include "Elliptic/DiscontinuousGalerkin/DgOperator.hpp"
#include "Elliptic/DiscontinuousGalerkin/SubdomainOperator/Tags.hpp"
#include "Elliptic/DiscontinuousGalerkin/Tags.hpp"
#include "Elliptic/Systems/GetSourcesComputer.hpp"
#include "Elliptic/Utilities/ApplyAt.hpp"
#include "NumericalAlgorithms/DiscontinuousGalerkin/MortarHelpers.hpp"
#include "NumericalAlgorithms/DiscontinuousGalerkin/Tags.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Parallel/PupStlCpp17.hpp"
#include "ParallelAlgorithms/LinearSolver/Schwarz/ElementCenteredSubdomainData.hpp"
#include "ParallelAlgorithms/LinearSolver/Schwarz/OverlapHelpers.hpp"
#include "ParallelAlgorithms/LinearSolver/Schwarz/SubdomainOperator.hpp"
#include "ParallelAlgorithms/LinearSolver/Schwarz/Tags.hpp"
#include "Utilities/EqualWithinRoundoff.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

/// Items related to the restriction of the DG operator to an element-centered
/// subdomain
namespace elliptic::dg::subdomain_operator {

namespace detail {
// Wrap the `Tag` in `LinearSolver::Schwarz::Tags::Overlaps`, except if it is
// included in `TakeFromCenterTags`.
template <typename Tag, typename Dim, typename OptionsGroup,
          typename TakeFromCenterTags>
struct make_overlap_tag_impl {
  using type = tmpl::conditional_t<
      tmpl::list_contains_v<TakeFromCenterTags, Tag>, Tag,
      LinearSolver::Schwarz::Tags::Overlaps<Tag, Dim::value, OptionsGroup>>;
};

// Wrap the `Tag` in `Tags::NeighborMortars`
template <typename Tag, typename Dim>
struct make_neighbor_mortars_tag_impl {
  using type = Tags::NeighborMortars<Tag, Dim::value>;
};
}  // namespace detail

/*!
 * \brief The elliptic DG operator on an element-centered subdomain
 *
 * This operator is a restriction of the full (linearized) DG-operator to an
 * element-centered subdomain with a few points overlap into neighboring
 * elements. It is a `LinearSolver::Schwarz::SubdomainOperator` to be used with
 * the Schwarz linear solver when it solves the elliptic DG operator.
 *
 * This operator requires the following tags are available on overlap regions
 * with neighboring elements:
 *
 * - Geometric quantities provided by
 *   `elliptic::dg::subdomain_operator::InitializeSubdomain`.
 * - All `System::fluxes_computer::argument_tags` and
 *   `System::sources_computer::argument_tags` (or
 *   `System::sources_computer_linearized::argument_tags` for nonlinear
 *   systems), except those listed in `ArgsTagsFromCenter`. The latter will be
 *   taken from the central element's DataBox, so they don't need to be made
 *   available on overlaps.
 * - The `System::fluxes_computer::argument_tags` on internal and external
 *   interfaces, except those listed in `System::fluxes_computer::volume_tags`.
 *
 * Some of these tags may require communication between elements. For example,
 * nonlinear system fields are constant background fields for the linearized DG
 * operator, but are updated in every nonlinear solver iteration. Therefore, the
 * updated nonlinear fields must be communicated across overlaps between
 * nonlinear solver iterations. To perform the communication you can use
 * `LinearSolver::Schwarz::Actions::SendOverlapFields` and
 * `LinearSolver::Schwarz::Actions::ReceiveOverlapFields`, setting
 * `RestrictToOverlap` to `false`. See
 * `LinearSolver::Schwarz::SubdomainOperator` for details.
 *
 * \par Overriding boundary conditions
 * Sometimes the subdomain operator should not use the boundary conditions that
 * have been selected when setting up the domain. For example, when the
 * subdomain operator is cached between non-linear solver iterations but the
 * boundary conditions depend on the non-linear fields, the preconditioning can
 * become ineffective (see
 * `LinearSolver::Schwarz::Actions::ResetSubdomainSolver`). Another example is
 * `elliptic::subdomain_preconditioners::MinusLaplacian`, where an auxiliary
 * Poisson system is used for preconditioning that doesn't have boundary
 * conditions set up in the domain. In these cases, the boundary conditions used
 * for the subdomain operator can be overridden with the optional
 * `override_boundary_conditions` argument to the `operator()`. If the
 * overriding boundary conditions are different from those listed in
 * `Metavariables::factory_creation`, you can supply the list of
 * boundary-condition classes to the `BoundaryConditionClasses` template
 * parameter. Note that the subdomain operator always applies the _linearized_
 * boundary conditions.
 *
 * \warning The subdomain operator hasn't been tested with periodic boundary
 * conditions so far.
 */
template <typename System, typename OptionsGroup,
          typename ArgsTagsFromCenter = tmpl::list<>,
          typename BoundaryConditionClasses = tmpl::list<>>
struct SubdomainOperator
    : LinearSolver::Schwarz::SubdomainOperator<System::volume_dim> {
 public:
  using system = System;
  using options_group = OptionsGroup;

 private:
  static constexpr size_t Dim = System::volume_dim;

  // Operator applications happen sequentially so we don't have to keep track of
  // the temporal id
  static constexpr size_t temporal_id = 0;

  // The subdomain operator always applies the linearized DG operator
  static constexpr bool linearized = true;

  using BoundaryConditionsBase = typename System::boundary_conditions_base;

  // These are the arguments that we need to retrieve from the DataBox and pass
  // to the functions in `elliptic::dg`, both on the central element and on
  // neighbors
  using prepare_args_tags =
      tmpl::list<domain::Tags::Element<Dim>, domain::Tags::Mesh<Dim>,
                 domain::Tags::InverseJacobian<Dim, Frame::ElementLogical,
                                               Frame::Inertial>,
                 domain::Tags::Faces<Dim, domain::Tags::FaceNormal<Dim>>,
                 domain::Tags::Faces<
                     Dim, domain::Tags::UnnormalizedFaceNormalMagnitude<Dim>>,
                 ::Tags::Mortars<domain::Tags::Mesh<Dim - 1>, Dim>,
                 ::Tags::Mortars<::Tags::MortarSize<Dim - 1>, Dim>>;
  using apply_args_tags = tmpl::list<
      domain::Tags::Element<Dim>, domain::Tags::Mesh<Dim>,
      domain::Tags::InverseJacobian<Dim, Frame::ElementLogical,
                                    Frame::Inertial>,
      domain::Tags::DetInvJacobian<Frame::ElementLogical, Frame::Inertial>,
      domain::Tags::Faces<Dim,
                          domain::Tags::UnnormalizedFaceNormalMagnitude<Dim>>,
      domain::Tags::Faces<Dim, domain::Tags::DetSurfaceJacobian<
                                   Frame::ElementLogical, Frame::Inertial>>,
      ::Tags::Mortars<domain::Tags::Mesh<Dim - 1>, Dim>,
      ::Tags::Mortars<::Tags::MortarSize<Dim - 1>, Dim>,
      ::Tags::Mortars<domain::Tags::DetSurfaceJacobian<Frame::ElementLogical,
                                                       Frame::Inertial>,
                      Dim>,
      elliptic::dg::Tags::PenaltyParameter, elliptic::dg::Tags::Massive>;
  using fluxes_args_tags = typename System::fluxes_computer::argument_tags;
  using sources_args_tags =
      typename elliptic::get_sources_computer<System,
                                              linearized>::argument_tags;

  // We need the fluxes args also on interfaces (internal and external). The
  // volume tags are the subset that don't have to be taken from interfaces.
  using fluxes_args_volume_tags = typename System::fluxes_computer::volume_tags;

  // These tags can be taken directly from the central element's DataBox, even
  // when evaluating neighbors
  using args_tags_from_center = tmpl::remove_duplicates<
      tmpl::push_back<ArgsTagsFromCenter, elliptic::dg::Tags::PenaltyParameter,
                      elliptic::dg::Tags::Massive>>;

  // Data on neighbors is stored in the central element's DataBox in
  // `LinearSolver::Schwarz::Tags::Overlaps` maps, so we wrap the argument tags
  // with this prefix
  using make_overlap_tag =
      detail::make_overlap_tag_impl<tmpl::_1, tmpl::pin<tmpl::size_t<Dim>>,
                                    tmpl::pin<OptionsGroup>,
                                    tmpl::pin<args_tags_from_center>>;
  using prepare_args_tags_overlap =
      tmpl::transform<prepare_args_tags, make_overlap_tag>;
  using apply_args_tags_overlap =
      tmpl::transform<apply_args_tags, make_overlap_tag>;
  using fluxes_args_tags_overlap =
      tmpl::transform<fluxes_args_tags, make_overlap_tag>;
  using sources_args_tags_overlap =
      tmpl::transform<sources_args_tags, make_overlap_tag>;
  using fluxes_args_tags_overlap_faces = tmpl::transform<
      domain::make_faces_tags<Dim, fluxes_args_tags, fluxes_args_volume_tags>,
      make_overlap_tag>;

  // We also need some data on the remote side of all neighbors' mortars. Such
  // data is stored in the central element's DataBox in `Tags::NeighborMortars`
  // maps
  using make_neighbor_mortars_tag =
      detail::make_neighbor_mortars_tag_impl<tmpl::_1,
                                             tmpl::pin<tmpl::size_t<Dim>>>;

 public:
  /// \warning This function is not thread-safe because it accesses mutable
  /// memory buffers.
  template <typename ResultTags, typename OperandTags, typename DbTagsList>
  void operator()(
      const gsl::not_null<
          LinearSolver::Schwarz::ElementCenteredSubdomainData<Dim, ResultTags>*>
          result,
      const LinearSolver::Schwarz::ElementCenteredSubdomainData<
          Dim, OperandTags>& operand,
      const db::DataBox<DbTagsList>& box,
      const std::unordered_map<std::pair<size_t, Direction<Dim>>,
                               const BoundaryConditionsBase&,
                               boost::hash<std::pair<size_t, Direction<Dim>>>>&
          override_boundary_conditions = {}) const {
    // Used to retrieve items out of the DataBox to forward to functions. This
    // replaces a long series of db::get calls.
    const auto get_items = [](const auto&... args) {
      return std::forward_as_tuple(args...);
    };

    // Retrieve data out of the DataBox
    using tags_to_retrieve = tmpl::flatten<tmpl::list<
        domain::Tags::Domain<Dim>, domain::Tags::Element<Dim>,
        ::Tags::Mortars<domain::Tags::Mesh<Dim - 1>, Dim>,
        // Data on overlaps with neighbors
        tmpl::transform<
            tmpl::flatten<tmpl::list<
                Tags::ExtrudingExtent, domain::Tags::Element<Dim>,
                domain::Tags::Mesh<Dim>,
                domain::Tags::Faces<
                    Dim, domain::Tags::UnnormalizedFaceNormalMagnitude<Dim>>,
                ::Tags::Mortars<domain::Tags::Mesh<Dim - 1>, Dim>,
                ::Tags::Mortars<::Tags::MortarSize<Dim - 1>, Dim>,
                // Data on the remote side of the neighbor's mortars
                tmpl::transform<
                    tmpl::list<
                        domain::Tags::Mesh<Dim>,
                        domain::Tags::UnnormalizedFaceNormalMagnitude<Dim>,
                        domain::Tags::Mesh<Dim - 1>,
                        ::Tags::MortarSize<Dim - 1>>,
                    make_neighbor_mortars_tag>>>,
            make_overlap_tag>>>;
    const auto& [domain, central_element, central_mortar_meshes,
                 all_overlap_extents, all_neighbor_elements,
                 all_neighbor_meshes,
                 all_neighbor_face_normal_magnitudes,
                 all_neighbor_mortar_meshes, all_neighbor_mortar_sizes,
                 all_neighbors_neighbor_meshes,
                 all_neighbors_neighbor_face_normal_magnitudes,
                 all_neighbors_neighbor_mortar_meshes,
                 all_neighbors_neighbor_mortar_sizes] =
        db::apply<tags_to_retrieve>(get_items, box);
    const auto fluxes_args = db::apply<fluxes_args_tags>(get_items, box);
    const auto sources_args = db::apply<sources_args_tags>(get_items, box);
    using FluxesArgs = std::decay_t<decltype(fluxes_args)>;
    DirectionMap<Dim, FluxesArgs> fluxes_args_on_faces{};
    for (const auto& direction : Direction<Dim>::all_directions()) {
      fluxes_args_on_faces.emplace(
          direction, elliptic::util::apply_at<
                         domain::make_faces_tags<Dim, fluxes_args_tags,
                                                 fluxes_args_volume_tags>,
                         fluxes_args_volume_tags>(get_items, box, direction));
    }

    // Setup boundary conditions
    const auto apply_boundary_condition =
        [&box, &local_domain = domain, &override_boundary_conditions](
            const ElementId<Dim>& local_element_id,
            const Direction<Dim>& local_direction, auto is_overlap,
            const auto& map_keys, const auto... fields_and_fluxes) {
          constexpr bool is_overlap_v =
              std::decay_t<decltype(is_overlap)>::value;
          // Get boundary conditions from domain, or use overridden boundary
          // conditions
          const auto& boundary_condition = [&local_domain, &local_element_id,
                                            &local_direction,
                                            &override_boundary_conditions]()
              -> const BoundaryConditionsBase& {
            if (not override_boundary_conditions.empty()) {
              const auto found_overridden_boundary_conditions =
                  override_boundary_conditions.find(
                      {local_element_id.block_id(), local_direction});
              ASSERT(found_overridden_boundary_conditions !=
                         override_boundary_conditions.end(),
                     "Overriding boundary conditions in subdomain operator, "
                     "but none is available for block "
                         << local_element_id.block_id() << " in direction "
                         << local_direction
                         << ". Make sure you have considered this external "
                            "boundary of the subdomain. If this is "
                            "intentional, add support to "
                            "elliptic::dg::SubdomainOperator.");
              return found_overridden_boundary_conditions->second;
            }
            const auto& boundary_conditions =
                local_domain.blocks()
                    .at(local_element_id.block_id())
                    .external_boundary_conditions();
            ASSERT(boundary_conditions.contains(local_direction),
                   "No boundary condition is available in block "
                       << local_element_id.block_id() << " in direction "
                       << local_direction
                       << ". Make sure you are setting up boundary conditions "
                          "when creating the domain.");
            ASSERT(
                dynamic_cast<const BoundaryConditionsBase*>(
                    boundary_conditions.at(local_direction).get()) != nullptr,
                "The boundary condition in block "
                    << local_element_id.block_id() << " in direction "
                    << local_direction
                    << " has an unexpected type. Make sure it derives off the "
                       "'boundary_conditions_base' class set in the system.");
            return dynamic_cast<const BoundaryConditionsBase&>(
                *boundary_conditions.at(local_direction));
          }();
          elliptic::apply_boundary_condition<
              linearized,
              tmpl::conditional_t<is_overlap_v, make_overlap_tag, void>,
              BoundaryConditionClasses>(boundary_condition, box, map_keys,
                                        fields_and_fluxes...);
        };

    // Check if the subdomain data is sparse, i.e. if some elements have zero
    // data. If they are, the operator is a lot cheaper to apply due to its
    // linearity.
    std::unordered_set<ElementId<Dim>> elements_in_subdomain{
        central_element.id()};
    std::unordered_set<ElementId<Dim>> elements_with_zero_data{};
    if (equal_within_roundoff(operand.element_data, 0.)) {
      elements_with_zero_data.insert(central_element.id());
    }
    for (const auto& [overlap_id, overlap_data] : operand.overlap_data) {
      elements_in_subdomain.insert(overlap_id.second);
      if (equal_within_roundoff(overlap_data, 0.)) {
        elements_with_zero_data.insert(overlap_id.second);
      }
    }
    const auto is_in_subdomain =
        [&elements_in_subdomain](const ElementId<Dim>& element_id) {
          return elements_in_subdomain.find(element_id) !=
                 elements_in_subdomain.end();
        };
    const auto data_is_zero = [&elements_with_zero_data, &is_in_subdomain](
                                  const ElementId<Dim>& element_id) {
      return elements_with_zero_data.find(element_id) !=
                 elements_with_zero_data.end() or
             // Data outside the subdomain is zero by definition
             not is_in_subdomain(element_id);
    };
    const bool central_data_is_zero = data_is_zero(central_element.id());

    // The subdomain operator essentially does two sweeps over all elements in
    // the subdomain: In the first sweep it prepares the mortar data and stores
    // them on both sides of all mortars, and in the second sweep it consumes
    // the mortar data to apply the operator. This implementation is relatively
    // simple because it can re-use the implementation for the parallel DG
    // operator. However, it is also possible to apply the subdomain operator in
    // a single sweep over all elements, incrementally building up the mortar
    // data and applying boundary corrections immediately to both adjacent
    // elements once the data is available. That approach is possibly a
    // performance optimization but requires re-implementing a lot of logic for
    // the DG operator here. It should be considered once the subdomain operator
    // has been identified as the performance bottleneck. An alternative to
    // optimizing the subdomain operator performance is to precondition the
    // subdomain solve with a _much_ simpler subdomain operator, such as a
    // finite-difference Laplacian, so fewer applications of the more expensive
    // DG subdomain operator are necessary.

    // 1. Prepare mortar data on all elements in the subdomain and store them on
    //    mortars, reorienting if needed
    //
    // Prepare central element
    const auto apply_boundary_condition_center =
        [&apply_boundary_condition, &local_central_element = central_element](
            const Direction<Dim>& local_direction,
            const auto... fields_and_fluxes) {
          apply_boundary_condition(local_central_element.id(), local_direction,
                                   std::false_type{}, local_direction,
                                   fields_and_fluxes...);
        };
    db::apply<prepare_args_tags>(
        [this, &operand](const auto&... args) {
          elliptic::dg::prepare_mortar_data<System, linearized>(
              make_not_null(&central_auxiliary_vars_),
              make_not_null(&central_auxiliary_fluxes_),
              make_not_null(&central_primal_fluxes_),
              make_not_null(&central_mortar_data_), operand.element_data,
              args...);
        },
        box, temporal_id, apply_boundary_condition_center, fluxes_args,
        sources_args, fluxes_args_on_faces, data_is_zero);
    // Prepare neighbors
    for (const auto& [direction, neighbors] : central_element.neighbors()) {
      const auto& orientation = neighbors.orientation();
      const auto direction_from_neighbor = orientation(direction.opposite());
      for (const auto& neighbor_id : neighbors) {
        const LinearSolver::Schwarz::OverlapId<Dim> overlap_id{direction,
                                                               neighbor_id};
        const auto& overlap_extent = all_overlap_extents.at(overlap_id);
        const auto& neighbor = all_neighbor_elements.at(overlap_id);
        const auto& neighbor_mesh = all_neighbor_meshes.at(overlap_id);
        const auto& mortar_id = overlap_id;
        const auto& mortar_mesh = central_mortar_meshes.at(mortar_id);
        const ::dg::MortarId<Dim> mortar_id_from_neighbor{
            direction_from_neighbor, central_element.id()};
        const bool neighbor_data_is_zero = data_is_zero(neighbor_id);

        // Intercept empty overlaps. In the unlikely case that overlaps have
        // zero extent, meaning no point of the neighbor is part of the
        // subdomain (which is fairly useless, except for testing), the
        // subdomain is identical to the central element and no communication
        // with neighbors is necessary. We can just handle the mortar between
        // central element and neighbor and continue.
        if (UNLIKELY(overlap_extent == 0)) {
          const auto& mortar_mesh_from_neighbor =
              all_neighbor_mortar_meshes.at(overlap_id)
                  .at(mortar_id_from_neighbor);
          const auto& mortar_size_from_neighbor =
              all_neighbor_mortar_sizes.at(overlap_id)
                  .at(mortar_id_from_neighbor);
          auto remote_boundary_data =
              elliptic::dg::zero_boundary_data_on_mortar<
                  typename System::primal_fields,
                  typename System::primal_fluxes>(
                  direction_from_neighbor, neighbor_mesh,
                  all_neighbor_face_normal_magnitudes.at(overlap_id)
                      .at(direction_from_neighbor),
                  mortar_mesh_from_neighbor, mortar_size_from_neighbor);
          if (not orientation.is_aligned()) {
            remote_boundary_data.orient_on_slice(
                mortar_mesh_from_neighbor.extents(),
                direction_from_neighbor.dimension(), orientation.inverse_map());
          }
          central_mortar_data_.at(mortar_id).remote_insert(
              temporal_id, std::move(remote_boundary_data));
          continue;
        }

        // Copy the central element's mortar data to the neighbor
        if (not(central_data_is_zero and neighbor_data_is_zero)) {
          auto oriented_mortar_data =
              central_mortar_data_.at(mortar_id).local_data(temporal_id);
          if (not orientation.is_aligned()) {
            oriented_mortar_data.orient_on_slice(
                mortar_mesh.extents(), direction.dimension(), orientation);
          }
          neighbors_mortar_data_[overlap_id][::dg::MortarId<Dim>{
                                                 direction_from_neighbor,
                                                 central_element.id()}]
              .remote_insert(temporal_id, std::move(oriented_mortar_data));
        }

        // Now we switch perspective to the neighbor. First, we extend the
        // overlap data to the full neighbor mesh by padding it with zeros. This
        // is necessary because spectral operators such as derivatives require
        // data on the full mesh.
        if (not neighbor_data_is_zero) {
          LinearSolver::Schwarz::extended_overlap_data(
              make_not_null(&extended_operand_vars_[overlap_id]),
              operand.overlap_data.at(overlap_id), neighbor_mesh.extents(),
              overlap_extent, direction_from_neighbor);
        }

        const auto apply_boundary_condition_neighbor =
            [&apply_boundary_condition, &local_neighbor_id = neighbor_id,
             &overlap_id](const Direction<Dim>& local_direction,
                          const auto... fields_and_fluxes) {
              apply_boundary_condition(
                  local_neighbor_id, local_direction, std::true_type{},
                  std::forward_as_tuple(overlap_id, local_direction),
                  fields_and_fluxes...);
            };

        const auto fluxes_args_on_overlap =
            elliptic::util::apply_at<fluxes_args_tags_overlap,
                                     args_tags_from_center>(get_items, box,
                                                            overlap_id);
        const auto sources_args_on_overlap =
            elliptic::util::apply_at<sources_args_tags_overlap,
                                     args_tags_from_center>(get_items, box,
                                                            overlap_id);
        DirectionMap<Dim, FluxesArgs> fluxes_args_on_overlap_faces{};
        for (const auto& neighbor_direction :
             Direction<Dim>::all_directions()) {
          fluxes_args_on_overlap_faces.emplace(
              neighbor_direction,
              elliptic::util::apply_at<fluxes_args_tags_overlap_faces,
                                       args_tags_from_center>(
                  get_items, box,
                  std::forward_as_tuple(overlap_id, neighbor_direction)));
        }

        elliptic::util::apply_at<prepare_args_tags_overlap,
                                 args_tags_from_center>(
            [this, &overlap_id](const auto&... args) {
              elliptic::dg::prepare_mortar_data<System, linearized>(
                  make_not_null(&neighbors_auxiliary_vars_[overlap_id]),
                  make_not_null(&neighbors_auxiliary_fluxes_[overlap_id]),
                  make_not_null(&neighbors_primal_fluxes_[overlap_id]),
                  make_not_null(&neighbors_mortar_data_[overlap_id]),
                  extended_operand_vars_[overlap_id], args...);
            },
            box, overlap_id, temporal_id, apply_boundary_condition_neighbor,
            fluxes_args_on_overlap, sources_args_on_overlap,
            fluxes_args_on_overlap_faces, data_is_zero);

        // Copy this neighbor's mortar data to the other side of the mortars. On
        // the other side we either have the central element, or another element
        // that may or may not be part of the subdomain.
        const auto& neighbor_mortar_meshes =
            all_neighbor_mortar_meshes.at(overlap_id);
        for (const auto& neighbor_mortar_id_and_data :
             neighbors_mortar_data_.at(overlap_id)) {
          // No structured bindings because capturing these in lambdas doesn't
          // work until C++20
          const auto& neighbor_mortar_id = neighbor_mortar_id_and_data.first;
          const auto& neighbor_mortar_data = neighbor_mortar_id_and_data.second;
          const auto& neighbor_direction = neighbor_mortar_id.first;
          const auto& neighbors_neighbor_id = neighbor_mortar_id.second;
          // No need to do anything on external boundaries
          if (neighbors_neighbor_id == ElementId<Dim>::external_boundary_id()) {
            continue;
          }
          const auto& neighbor_orientation =
              neighbor.neighbors().at(neighbor_direction).orientation();
          const auto neighbors_neighbor_direction =
              neighbor_orientation(neighbor_direction.opposite());
          const ::dg::MortarId<Dim> mortar_id_from_neighbors_neighbor{
              neighbors_neighbor_direction, neighbor_id};
          const auto send_mortar_data =
              [&neighbor_orientation, &neighbor_mortar_meshes,
               &neighbor_mortar_data, &neighbor_mortar_id, &neighbor_direction,
               &neighbor_data_is_zero,
               &data_is_zero](auto& remote_mortar_data,
                              const ElementId<Dim>& remote_element_id) {
                if (neighbor_data_is_zero and data_is_zero(remote_element_id)) {
                  return;
                }
                const auto& neighbor_mortar_mesh =
                    neighbor_mortar_meshes.at(neighbor_mortar_id);
                auto oriented_neighbor_mortar_data =
                    neighbor_mortar_data.local_data(temporal_id);
                if (not neighbor_orientation.is_aligned()) {
                  oriented_neighbor_mortar_data.orient_on_slice(
                      neighbor_mortar_mesh.extents(),
                      neighbor_direction.dimension(), neighbor_orientation);
                }
                remote_mortar_data.remote_insert(
                    temporal_id, std::move(oriented_neighbor_mortar_data));
              };
          if (neighbors_neighbor_id == central_element.id() and
              mortar_id_from_neighbors_neighbor == mortar_id) {
            send_mortar_data(central_mortar_data_.at(mortar_id),
                             central_element.id());
            continue;
          }
          // Determine whether the neighbor's neighbor overlaps with the
          // subdomain and find its overlap ID if it does.
          const auto neighbors_neighbor_overlap_id =
              [&local_all_neighbor_mortar_meshes = all_neighbor_mortar_meshes,
               &neighbors_neighbor_id, &mortar_id_from_neighbors_neighbor,
               &is_in_subdomain]()
              -> std::optional<LinearSolver::Schwarz::OverlapId<Dim>> {
            if (not is_in_subdomain(neighbors_neighbor_id)) {
              return std::nullopt;
            }
            for (const auto& [local_overlap_id, local_mortar_meshes] :
                 local_all_neighbor_mortar_meshes) {
              if (local_overlap_id.second != neighbors_neighbor_id) {
                continue;
              }
              for (const auto& local_mortar_id_and_mesh : local_mortar_meshes) {
                if (local_mortar_id_and_mesh.first ==
                    mortar_id_from_neighbors_neighbor) {
                  return local_overlap_id;
                }
              }
            }
            ERROR("The neighbor's neighbor "
                  << neighbors_neighbor_id
                  << " is part of the subdomain, but we didn't find its "
                     "overlap ID. This is a bug, so please file an issue.");
          }();
          if (neighbors_neighbor_overlap_id.has_value()) {
            // The neighbor's neighbor is part of the subdomain so we copy the
            // mortar data over. Once the loop is complete we will also have
            // received mortar data back. At that point, both neighbors have a
            // copy of each other's mortar data, which is the subject of the
            // possible optimizations mentioned above. Note that the data may
            // differ by orientations.
            send_mortar_data(
                neighbors_mortar_data_[*neighbors_neighbor_overlap_id]
                                      [mortar_id_from_neighbors_neighbor],
                neighbors_neighbor_overlap_id->second);
          } else if (not neighbor_data_is_zero) {
            // The neighbor's neighbor does not overlap with the subdomain, so
            // we don't copy mortar data and also don't expect to receive any.
            // Instead, we assume the data on it is zero and manufacture
            // appropriate remote boundary data.
            const auto& neighbors_neighbor_mortar_mesh =
                all_neighbors_neighbor_mortar_meshes.at(overlap_id)
                    .at(neighbor_mortar_id);
            auto zero_mortar_data = elliptic::dg::zero_boundary_data_on_mortar<
                typename System::primal_fields, typename System::primal_fluxes>(
                neighbors_neighbor_direction,
                all_neighbors_neighbor_meshes.at(overlap_id)
                    .at(neighbor_mortar_id),
                all_neighbors_neighbor_face_normal_magnitudes.at(overlap_id)
                    .at(neighbor_mortar_id),
                neighbors_neighbor_mortar_mesh,
                all_neighbors_neighbor_mortar_sizes.at(overlap_id)
                    .at(neighbor_mortar_id));
            // The data is zero, but auxiliary quantities such as the face
            // normal magnitude may need re-orientation
            if (not neighbor_orientation.is_aligned()) {
              zero_mortar_data.orient_on_slice(
                  neighbors_neighbor_mortar_mesh.extents(),
                  neighbors_neighbor_direction.dimension(),
                  neighbor_orientation.inverse_map());
            }
            neighbors_mortar_data_.at(overlap_id)
                .at(neighbor_mortar_id)
                .remote_insert(temporal_id, std::move(zero_mortar_data));
          }
        }  // loop over neighbor's mortars
      }    // loop over neighbors
    }      // loop over directions

    // 2. Apply the operator on all elements in the subdomain
    //
    // Apply on central element
    db::apply<apply_args_tags>(
        [this, &result, &operand](const auto&... args) {
          elliptic::dg::apply_operator<System, linearized>(
              make_not_null(&result->element_data),
              make_not_null(&central_mortar_data_), operand.element_data,
              central_primal_fluxes_, args...);
        },
        box, temporal_id, sources_args, data_is_zero);
    // Apply on neighbors
    for (const auto& [direction, neighbors] : central_element.neighbors()) {
      const auto& orientation = neighbors.orientation();
      const auto direction_from_neighbor = orientation(direction.opposite());
      for (const auto& neighbor_id : neighbors) {
        const LinearSolver::Schwarz::OverlapId<Dim> overlap_id{direction,
                                                               neighbor_id};
        const auto& overlap_extent = all_overlap_extents.at(overlap_id);
        const auto& neighbor_mesh = all_neighbor_meshes.at(overlap_id);

        if (UNLIKELY(overlap_extent == 0)) {
          continue;
        }

        elliptic::util::apply_at<apply_args_tags_overlap,
                                 args_tags_from_center>(
            [this, &overlap_id](const auto&... args) {
              elliptic::dg::apply_operator<System, linearized>(
                  make_not_null(&extended_results_[overlap_id]),
                  make_not_null(&neighbors_mortar_data_.at(overlap_id)),
                  extended_operand_vars_.at(overlap_id),
                  neighbors_primal_fluxes_.at(overlap_id), args...);
            },
            box, overlap_id, temporal_id,
            elliptic::util::apply_at<sources_args_tags_overlap,
                                     args_tags_from_center>(get_items, box,
                                                            overlap_id),
            data_is_zero);

        // Restrict the extended operator data back to the subdomain, assuming
        // we can discard any data outside the overlaps. WARNING: This
        // assumption may break with changes to the DG operator that affect its
        // sparsity. For example, multiplying the DG operator with the _full_
        // inverse mass-matrix ("massless" scheme with no "mass-lumping"
        // approximation) means that lifted boundary corrections bleed into the
        // volume.
        if (UNLIKELY(
                result->overlap_data[overlap_id].number_of_grid_points() !=
                operand.overlap_data.at(overlap_id).number_of_grid_points())) {
          result->overlap_data[overlap_id].initialize(
              operand.overlap_data.at(overlap_id).number_of_grid_points());
        }
        LinearSolver::Schwarz::data_on_overlap(
            make_not_null(&result->overlap_data[overlap_id]),
            extended_results_.at(overlap_id), neighbor_mesh.extents(),
            overlap_extent, direction_from_neighbor);
      }  // loop over neighbors
    }    // loop over directions
  }

  // NOLINTNEXTLINE(google-runtime-references)
  void pup(PUP::er& /*p*/) {}

 private:
  // Memory buffers for repeated operator applications
  // NOLINTNEXTLINE(spectre-mutable)
  mutable Variables<typename System::auxiliary_fields>
      central_auxiliary_vars_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable Variables<typename System::primal_fluxes> central_primal_fluxes_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable Variables<typename System::auxiliary_fluxes>
      central_auxiliary_fluxes_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable LinearSolver::Schwarz::OverlapMap<
      Dim, Variables<typename System::auxiliary_fields>>
      neighbors_auxiliary_vars_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable LinearSolver::Schwarz::OverlapMap<
      Dim, Variables<typename System::primal_fluxes>>
      neighbors_primal_fluxes_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable LinearSolver::Schwarz::OverlapMap<
      Dim, Variables<typename System::auxiliary_fluxes>>
      neighbors_auxiliary_fluxes_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable LinearSolver::Schwarz::OverlapMap<
      Dim, Variables<typename System::primal_fields>>
      extended_operand_vars_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable LinearSolver::Schwarz::OverlapMap<
      Dim, Variables<typename System::primal_fields>>
      extended_results_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable ::dg::MortarMap<
      Dim, elliptic::dg::MortarData<size_t, typename System::primal_fields,
                                    typename System::primal_fluxes>>
      central_mortar_data_{};
  // NOLINTNEXTLINE(spectre-mutable)
  mutable LinearSolver::Schwarz::OverlapMap<
      Dim, ::dg::MortarMap<Dim, elliptic::dg::MortarData<
                                    size_t, typename System::primal_fields,
                                    typename System::primal_fluxes>>>
      neighbors_mortar_data_{};
};

}  // namespace elliptic::dg::subdomain_operator


/*
==== Author:

Relja Arandjelovic (relja@robots.ox.ac.uk)
Visual Geometry Group,
Department of Engineering Science
University of Oxford

==== Copyright:

The library belongs to Relja Arandjelovic and the University of Oxford.
No usage or redistribution is allowed without explicit permission.
*/

#include "det_ransac.h"

#include <Eigen/Dense>

#include "putative.h"

inline double mysqr( double x ){ return x*x; }

double
detRansac::matchWords_Hard(
    sameRandomUint32 const &sameRandomObj,

    uint32_t &nInliers,

    std::vector<quantDesc> const &ids1,
    std::vector<ellipse> const &ellipses1,
    std::vector<uint32_t> const *presortedInds1,
    std::vector<quantDesc> const &ids2,
    std::vector<ellipse> const &ellipses2,

    double errorThr,
    double lowAreaChange, double highAreaChange,
    uint32_t nReest,

    homography *H,
    matchesType *inlierInds

    ){

    std::vector<uint32_t> ids1_flat, ids2_flat;
    quantDesc::flattenHard( ids1, ids1_flat );
    quantDesc::flattenHard( ids2, ids2_flat );

    std::vector< std::pair<uint32_t, uint32_t> > putativeMatches;
    putative_quantized::getPutativeMatches_Hard( ids1_flat, ids2_flat, putativeMatches, presortedInds1 );

    return detRansac::match( sameRandomObj, nInliers, ellipses1, ellipses2, putativeMatches, NULL, errorThr, lowAreaChange, highAreaChange, nReest, H, inlierInds );

}

double
detRansac::matchWords_Soft(
    sameRandomUint32 const &sameRandomObj,

    uint32_t &nInliers,

    std::vector<quantDesc> const &ids1,
    std::vector<ellipse> const &ellipses1,
    std::vector<uint32_t> const *presortedInds1,
    std::vector<quantDesc> const &ids2,
    std::vector<ellipse> const &ellipses2,

    double errorThr,
    double lowAreaChange, double highAreaChange,
    uint32_t nReest,

    homography *H,
    matchesType *inlierInds

    ){

    std::vector<wordWeightPair> ids1_flat, ids2_flat;
    std::vector<uint32_t> qDInds1, qDInds2;
    quantDesc::flatten( ids1, ids1_flat, &qDInds1, true );
    quantDesc::flatten( ids2, ids2_flat, &qDInds2, true );

    std::vector< std::pair<uint32_t, uint32_t> > putativeMatches;
    std::vector<double> PMweights;
    putative_quantized::getPutativeMatches_Soft( ids1_flat, qDInds1, ids2_flat, qDInds2, putativeMatches, PMweights, presortedInds1 );

//     return detRansac::match( sameRandomObj, nInliers, ellipses1, ellipses2, putativeMatches, NULL, errorThr, lowAreaChange, highAreaChange, nReest, H, inlierInds );
    return detRansac::match( sameRandomObj, nInliers, ellipses1, ellipses2, putativeMatches, &PMweights, errorThr, lowAreaChange, highAreaChange, nReest, H, inlierInds );

}

double
detRansac::matchDesc(
    sameRandomUint32 const &sameRandomObj,

    uint32_t &nInliers,

    float const* desc1,
    std::vector<ellipse> const &ellipses1,
    float const* desc2,
    std::vector<ellipse> const &ellipses2,
    uint32_t nDims,

    double errorThr,
    double lowAreaChange, double highAreaChange,
    uint32_t nReest,

    bool useLowe,
    float deltaSq,
    float epsilon,

    homography *H,
    matchesType *inlierInds

    ){

    std::vector< std::pair<uint32_t, uint32_t> > putativeMatches;

    putative_desc<float>::getPutativeMatches( desc1, ellipses1.size(), desc2, ellipses2.size(), nDims, putativeMatches, useLowe, deltaSq, epsilon );

    return detRansac::match( sameRandomObj, nInliers, ellipses1, ellipses2, putativeMatches, NULL, errorThr, lowAreaChange, highAreaChange, nReest, H, inlierInds);

}

double
detRansac::match(
    sameRandomUint32 const &sameRandomObj,

    uint32_t &bestNInliers,

    std::vector<ellipse> const &ellipses1,
    std::vector<ellipse> const &ellipses2,
    matchesType const &putativeMatches,
    std::vector<double> const *PMweights,

    double errorThr,
    double lowAreaChange, double highAreaChange,
    uint32_t nReest,

    homography *H,
    matchesType *inlierInds

    ){

    bestNInliers= 0;

    uint32_t nPutativeMatches= putativeMatches.size();

    if (nPutativeMatches<3)
        return 0;

    //------- prepare

    bool delWeights= false;
    if (PMweights==NULL) {
        PMweights= new std::vector<double>( nPutativeMatches, 1.0 );
        delWeights= true;
    }

    //------- generate Hs

    detRansac::inlierFinder inlierFinder_obj( ellipses1, ellipses2, putativeMatches, *PMweights, errorThr, lowAreaChange, highAreaChange );

    // initialize with identity
    homography Hident; Hident.setIdentity();
    homography bestH= Hident;

    double bestScore= 0.0;
    bestNInliers= 0;
    bestScore= inlierFinder_obj.getScore( bestH, bestNInliers, NULL );

    std::vector<homography> Hs;
    Hs.reserve( nPutativeMatches );

    for (std::vector< std::pair<uint32_t, uint32_t> >::const_iterator itPM= putativeMatches.begin();
         itPM!=putativeMatches.end();
         ++itPM){

        Hs.push_back( homography( ellipses1[itPM->first], ellipses2[itPM->second] ) );

    }

    uint32_t globalNIter= 0;

    sameRandomObj.shuffle<homography>( Hs.begin(), Hs.end() );

    {
        //------- RANSAC core

        static const double pFail= 0.001;

        uint32_t nInliers, iH= 0;
        double score;

        for (std::vector<homography>::const_iterator itH= Hs.begin();
             itH!=Hs.end() && globalNIter < detRansac::getNStopping(pFail, nPutativeMatches, bestNInliers);
             ++itH, ++globalNIter, ++iH){

            score= inlierFinder_obj.getScore( *itH, nInliers, NULL );

            if (nInliers>3 && score>bestScore) {
                bestNInliers= nInliers;
                bestScore= score;
                bestH= *itH;
            }

        }

    }

    if (bestNInliers > 3){

        matchesType bestInliers;
        bestInliers.reserve(bestNInliers);
        uint32_t nInliers_new;
        inlierFinder_obj.getScore( bestH, nInliers_new, &bestInliers );

        //------- reestimate

        homography H_new= bestH;
        double score_new;
        matchesType inliers_new= bestInliers;

        uint32_t iReest;
        for (iReest= 0; nInliers_new>3 && iReest<nReest; ++iReest){
            detRansac::getH( ellipses1, ellipses2, inliers_new, H_new );
            score_new= inlierFinder_obj.getScore( H_new, nInliers_new, &inliers_new );
            if (nInliers_new>3 && score_new>bestScore){
                bestScore= score_new;
                bestNInliers= nInliers_new;
                bestH= H_new;
                bestInliers= inliers_new; // don't swap as getH in next iteration needs it!
            }
        }

        if (inlierInds)
            inlierInds->swap( bestInliers );

        if (H)
            *H= bestH;

    }

    //------- cleanup

    if (delWeights)
        delete PMweights;

    return bestScore;

}

void
detRansac::getH( std::vector<ellipse> const &ellipses1,
                 std::vector<ellipse> const &ellipses2,
                 matchesType const &inliers, homography &H ){

    Eigen::Matrix<double, Eigen::Dynamic, 7> A( inliers.size()*2, 7 );

    double *x1, *y1, *x2, *y2, *_temp;
    ellipse::getCentres( ellipses1, ellipses2, inliers, x1, y1, x2, y2, _temp );
    uint32_t nInliers= inliers.size();

    // normalize points
    homography Hnorm1, Hnorm2;
    normPoints(x1, y1, nInliers, Hnorm1);
    normPoints(x2, y2, nInliers, Hnorm2);
    double Hnorm2inv[9];
    Hnorm2.getInverse(Hnorm2inv);

    // fit homography
    uint32_t i=0;

    for (matchesType::const_iterator itIn= inliers.begin();
         itIn!=inliers.end();
         ++itIn, ++i){

        A.coeffRef( i*2   , 0 )=    0.0;
        A.coeffRef( i*2   , 1 )=    0.0;
        A.coeffRef( i*2   , 2 )=    0.0;
        A.coeffRef( i*2   , 3 )=  x1[i];
        A.coeffRef( i*2   , 4 )=  y1[i];
        A.coeffRef( i*2   , 5 )=    1.0;
        A.coeffRef( i*2   , 6 )= -y2[i];

        A.coeffRef( i*2+1 , 0 )= -x1[i];
        A.coeffRef( i*2+1 , 1 )= -y1[i];
        A.coeffRef( i*2+1 , 2 )=   -1.0;
        A.coeffRef( i*2+1 , 3 )=    0.0;
        A.coeffRef( i*2+1 , 4 )=    0.0;
        A.coeffRef( i*2+1 , 5 )=    0.0;
        A.coeffRef( i*2+1 , 6 )=  x2[i];

    }

    delete []x1; delete []y1; delete []x2; delete []y2; delete []_temp;

    typedef Eigen::Matrix<double, 7, 7> matrix7x7;
    matrix7x7 AtA;
    AtA.noalias()= A.transpose() * A;
    Eigen::SelfAdjointEigenSolver<matrix7x7> sol( AtA );
    Eigen::Matrix<double,7,1> x= sol.eigenvectors().col(0);

    H.H[0]= x.coeff(0,0); H.H[1]= x.coeff(1,0); H.H[2]= x.coeff(2,0);
    H.H[3]= x.coeff(3,0); H.H[4]= x.coeff(4,0); H.H[5]= x.coeff(5,0);
    H.H[6]=          0.0; H.H[7]=          0.0; H.H[8]= x.coeff(6,0);
    H.normLast();

    // denormalize H: H= Hnorm2inv * Hnorm * Hnorm1

    Eigen::Matrix<double, 3, 3> Hnorm2inv_, Hnorm1_, Hnorm_;
    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j){
            Hnorm_.coeffRef(i,j)= H.H[i*3+j];
            Hnorm1_.coeffRef(i,j)= Hnorm1.H[i*3+j];
            Hnorm2inv_.coeffRef(i,j)= Hnorm2inv[i*3+j];
        }
    Eigen::Matrix<double, 3, 3> H_;
    H_.noalias()= Hnorm2inv_ * Hnorm_ * Hnorm1_;

    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j)
            H.H[i*3+j]= H_.coeff(i,j);

#if 0
    // this is how it used to be done in OpenCV, not updating though so it might be obsolete..

    cv::Mat A( inliers.size()*2, 7, cv::DataType<double>::type ), x;

    double *x1, *y1, *x2, *y2, *_temp;
    ellipse::getCentres( ellipses1, ellipses2, inliers, x1, y1, x2, y2, _temp );
    uint32_t nInliers= inliers.size();

    // normalize points
    homography Hnorm1, Hnorm2;
    normPoints(x1, y1, nInliers, Hnorm1);
    normPoints(x2, y2, nInliers, Hnorm2);
    double Hnorm2inv[9];
    Hnorm2.getInverse(Hnorm2inv);

    // fit homography
    uint32_t i=0;

    for (matchesType::const_iterator itIn= inliers.begin();
         itIn!=inliers.end();
         ++itIn, ++i){

        // normalize transform

        A.at<double>( i*2   , 0 )=    0.0;
        A.at<double>( i*2   , 1 )=    0.0;
        A.at<double>( i*2   , 2 )=    0.0;
        A.at<double>( i*2   , 3 )=  x1[i];
        A.at<double>( i*2   , 4 )=  y1[i];
        A.at<double>( i*2   , 5 )=    1.0;
        A.at<double>( i*2   , 6 )= -y2[i];

        A.at<double>( i*2+1 , 0 )= -x1[i];
        A.at<double>( i*2+1 , 1 )= -y1[i];
        A.at<double>( i*2+1 , 2 )=   -1.0;
        A.at<double>( i*2+1 , 3 )=    0.0;
        A.at<double>( i*2+1 , 4 )=    0.0;
        A.at<double>( i*2+1 , 5 )=    0.0;
        A.at<double>( i*2+1 , 6 )=  x2[i];

    }

    delete []x1; delete []y1; delete []x2; delete []y2; delete []_temp;

    cv::SVD::solveZ(A, x);

    H.H[0]= x.at<double>(0,0); H.H[1]= x.at<double>(1,0); H.H[2]= x.at<double>(2,0);
    H.H[3]= x.at<double>(3,0); H.H[4]= x.at<double>(4,0); H.H[5]= x.at<double>(5,0);
    H.H[6]=               0.0; H.H[7]=               0.0; H.H[8]= x.at<double>(6,0);
    H.normLast();

    // denormalize H: H= Hnorm2inv * Hnorm * Hnorm1

    cv::Mat Hnorm2inv_(3,3, cv::DataType<double>::type),
            Hnorm1_(3,3, cv::DataType<double>::type),
            Hnorm_(3,3, cv::DataType<double>::type);
    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j){
            Hnorm_.at<double>(i,j)= H.H[i*3+j];
            Hnorm1_.at<double>(i,j)= Hnorm1.H[i*3+j];
            Hnorm2inv_.at<double>(i,j)= Hnorm2inv[i*3+j];
        }
    cv::Mat H_= Hnorm2inv_ * Hnorm_ * Hnorm1_;

    for (int i=0; i<3; ++i)
        for (int j=0; j<3; ++j)
            H.H[i*3+j]= H_.at<double>(i,j);
#endif

}

void
detRansac::normPoints( double *x, double *y, uint32_t n, homography &Hnorm ){

    double EX= 0, EY= 0, stdX, stdY, EX2= 0, EY2= 0;
    uint32_t i;

    // get mean/variance
    for (i= 0; i<n; ++i){
        EX+= x[i];
        EY+= y[i];
        EX2+= mysqr(x[i]);
        EY2+= mysqr(y[i]);
    }
    EX/=n; EY/=n;
    EX2/=n; EY2/=n;
    // max is there just in case std=0 (as later norm=NaN in this case), in this case the dimension gets mapped to 0 (as mean is 0) so scaling makes no difference.. This is not nice however, as it means the problem is illposed
    stdX= std::max( sqrt(EX2-mysqr(EX)), 1e-4 );
    stdY= std::max( sqrt(EY2-mysqr(EY)), 1e-4 );

    double normX= sqrt(2.0)/stdX, normY= sqrt(2.0)/stdY;

    Hnorm.setIdentity();
    Hnorm.H[0]= normX; Hnorm.H[2]= -EX*normX;
    Hnorm.H[4]= normY; Hnorm.H[5]= -EY*normY;

    // normalize points
    for (i= 0; i<n; ++i){
        x[i]= (x[i]-EX)*normX;
        y[i]= (y[i]-EY)*normY;
    }

}

detRansac::inlierFinder::inlierFinder(
    std::vector<ellipse> const &aEllipses1,
    std::vector<ellipse> const &aEllipses2,
    matchesType const &aPutativeMatches,
    std::vector<double> const &aPMweights,
    double aErrorThr,
    double aLowAreaChange, double aHighAreaChange) :

    nIter(0),
    putativeMatches(&aPutativeMatches), PMweights(&aPMweights),
    errorThrSq(mysqr(aErrorThr)),
    lowAreaChangeSq(mysqr(aLowAreaChange)),
    highAreaChangeSq(mysqr(aHighAreaChange)),
    nPutativeMatches(aPutativeMatches.size()) {

    //------- get largest pIDs

    uint32_t maxpID1= 0, maxpID2= 0;

    for (matchesType::const_iterator itPM=putativeMatches->begin();
         itPM != putativeMatches->end();
         ++itPM){
        maxpID1= std::max(maxpID1, itPM->first);
        maxpID2= std::max(maxpID2, itPM->second);
    }
    point1Used.clear(); point1Used.resize(maxpID1+1,0);
    point1Used.clear(); point2Used.resize(maxpID2+1,0);

    ellipse::getCentres( aEllipses1, aEllipses2, *putativeMatches, x1, y1, x2, y2, areaDiffSq );

}

detRansac::inlierFinder::~inlierFinder(){
    delete []x1; delete []y1; delete []x2; delete []y2; delete []areaDiffSq;
}

double
detRansac::inlierFinder::getScore( homography const &H, uint32_t &nInliers, matchesType *inliers ){

    double score, detASq;

    score= 0.0;
    nInliers= 0;
    if (inliers)
        inliers->clear();

    detASq= mysqr( H.getDetAffine() );
    if (detASq<1e-4) return 0.0;

    double error;
    uint32_t pID1, pID2, iPM;
    double x, y, xi, yi;
    double lowAreaChangeSqByD =  lowAreaChangeSq / detASq;
    double highAreaChangeSqByD= highAreaChangeSq / detASq;

    double Hinv[9];
    H.getInverse( Hinv );

    ++nIter;

    iPM= 0;
    for (matchesType::const_iterator itPM= putativeMatches->begin();
         itPM!=putativeMatches->end();
         ++itPM, ++iPM){

        pID1= itPM->first;
        pID2= itPM->second;
        if (point1Used[ pID1 ]==nIter || point2Used[ pID2 ]==nIter)
            continue;

        homography::affTransform( H.H , x1[iPM], y1[iPM], x , y );
        homography::affTransform( Hinv, x2[iPM], y2[iPM], xi, yi );

        error= mysqr( x1[iPM]-xi ) + mysqr( y1[iPM]-yi ) +
               mysqr( x2[iPM]-x  ) + mysqr( y2[iPM]-y  );

        if (error < errorThrSq){
            /*
            areaChangeSq= detASq * ellipses2[ pID2 ].getPropAreaSq() / ellipses1[ pID1 ].getPropAreaSq();
            if ( areaChangeSq > lowAreaChangeSq && areaChangeSq < highAreaChangeSq ){
            */
            if (areaDiffSq[iPM] > lowAreaChangeSqByD && areaDiffSq[iPM] < highAreaChangeSqByD) {
                score+= PMweights->at(iPM);
                ++nInliers;
                point1Used[ pID1 ]= nIter;
                point2Used[ pID2 ]= nIter;
                if (inliers)
                    inliers->push_back(std::make_pair(pID1,pID2));
            }
        }

    }

    return score;

}