#ifndef __METROPOLIS_HPP__
#define __METROPOLIS_HPP__

#include <vector>
#include <array>
#include <random>
#include <functional>
#include <limits>
#include <armadillo>
#include <exception>
#include <initializer_list>
#include "pauth_types.hpp"
#include "molecular.hpp"
#include "potentials.hpp"
#include "acceptance.hpp"
#include "boundary.hpp"
#include "distance.hpp"
#include "trial.hpp"
#include "seed.hpp"

/* TODO: make random number generator more general so that user can specify
 *       which random number generator they'd prefer to use
 */

namespace pauth {

static const double _default_kB = 1.0;

static metric _default_metric = periodic_euclidean;
static bc _default_bc = periodic_bc;
static acc _default_acc = metropolis_acc;

class metropolis {
public:
  static metric DEFAULT_METRIC;
  static bc DEFAULT_BC;
  static acc DEFAULT_ACC;

  /*! \brief Default constructor
   */
  metropolis() {}

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     id          Molecular id of simulation molecules
   * \param     N           Number of molecules
   * \param     D           Number of dimensions
   * \param     L           Simulation box edge length
   * \param     tmg         Trial move generator
   * \param     pot         Molecular potential
   * \param     T           Simulation temperature
   * \param     kB          Boltzmann's constant
   * \param     m           Metric for measuring molecule-molecule distances
   * \param     bc          Boundary conditions
   * \param     acceptance  Determines whether a move is accepted for rejected
   * \param     sg          Function for generating a seed
   * \param     init_zeros  If true, initializes all molecules to position zero
   * \return                Markov chain Monte Carlo simulation object
   */
  metropolis(const molecular_id id, const size_t N, const size_t D,
             const double L, trial_move_generator tmg,
             abstract_potential* pot, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen,
             const bool init_zeros = false);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     id            Molecular id of simulation molecules
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pot           Molecular potential
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const molecular_id id, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             abstract_potential* pot, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pot           Molecular potential
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             abstract_potential* pot, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     id          Molecular id of simulation molecules
   * \param     N           Number of molecules
   * \param     D           Number of dimensions
   * \param     L           Simulation box edge length
   * \param     tmg         Trial move generator
   * \param     pots        Molecular potentials
   * \param     T           Simulation temperature
   * \param     kB          Boltzmann's constant
   * \param     m           Metric for measuring molecule-molecule distances
   * \param     bc          Boundary conditions
   * \param     acceptance  Determines whether a move is accepted for rejected
   * \param     sg          Function for generating a seed
   * \param     init_zeros  If true, initializes all molecules to position zero
   * \return                Markov chain Monte Carlo simulation object
   */
  metropolis(const molecular_id id, const size_t N, const size_t D,
             const double L, trial_move_generator tmg,
             std::initializer_list<abstract_potential*> pots, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen,
             const bool init_zeros = false);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     id            Molecular id of simulation molecules
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pots        Molecular potentials
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const molecular_id id, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             std::initializer_list<abstract_potential*> pots, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Constructor for a Markov chain Monte Carlo simulation
   *
   * \param     fname         Filename of initial molecular positions
   * \param     N             Number of molecules
   * \param     D             Number of dimensions
   * \param     L             Simulation box edge length
   * \param     tmg           Trial move generator
   * \param     pots        Molecular potentials
   * \param     T             Simulation temperature
   * \param     kB            Boltzmann's constant
   * \param     m             Metric for measuring molecule-molecule distances
   * \param     bc            Boundary conditions
   * \param     acceptance    Determines whether a move is accepted for rejected
   * \param     sg            Function for generating a seed
   * \return                  Markov chain Monte Carlo simulation object
   */
  metropolis(const char *fname, const size_t N,
             const size_t D, const double L, trial_move_generator tmg,
             std::initializer_list<abstract_potential*> pots, const double T,
             const double kB = _default_kB,
             metric m = _default_metric, bc boundary = _default_bc,
             acc acceptance = _default_acc,
             seed_gen sg = _default_seed_gen);

  /*! \brief Copy constructor
   *
   * \param     sim         Metropolis simulation object
   * \param     sg          Function for generating a seed
   * \return                Copy
   */
  metropolis(const metropolis &sim, seed_gen sg = _default_seed_gen)
    : _molecular_ids(sim._molecular_ids), _positions(sim._positions),
      _edge_lengths(sim._edge_lengths), _V(sim._V), _potentials(sim._potentials),
      _T(sim._T), _kB(sim._kB), _beta(sim._beta), _m(sim._m), _bc(sim._bc),
      _eps_dist(0.0, 1.0), _choice_dist(0, sim._molecular_ids.size()-1),
      _tmg(sim._tmg), _acc(sim._acc),
      _parallel_callbacks(sim._parallel_callbacks),
      _sequential_callbacks(sim._sequential_callbacks), _step(sim._step),
      _dx(sim._dx), _choice(sim._choice), _dU(sim._dU), _U(sim._U), _eps(sim._eps),
      _accepted(sim._accepted) { _rng.seed(sg()); }

  /*! \brief Assignment operator
   *
   * \param     rhs     Metropolis simulation object
   * \return            Self
   */
  metropolis operator=(const metropolis &rhs);

  /*! \brief Update the (cached) system energy
   */
  void update_U();

  /*! \brief Get number of molecules
   *
   * \return      Number of molecules
   */
  inline auto N() const { return _molecular_ids.size(); }

  /*! \brief Get number of dimensions
   *
   * \return      Number of dimensions
   */
  inline auto D() const { return _positions.n_rows; }

  /*! \brief Get molecular ids
   *
   * \return      Container of molecular ids
   */
  inline const auto &molecular_ids() const { return _molecular_ids; }

  /*! \brief Get molecular positions
   *
   * \return      Molecular positions
   */
  inline auto &positions() { return _positions; }

  /*! \brief Get molecular positions
   *
   * \return      Molecular positions
   */
  inline const auto &positions() const { return _positions; }

  /*! \brief Get Boltzmann's constant
   *
   * \return      Boltzmann's constant
   */
  inline auto kB() const { return _kB; }

  /*! \brief Get temperature
   *
   * \return      Temperature
   */
  inline auto T() const { return _T; }

  /*! \brief Get beta
   *
   * \return      1 / kT
   */
  inline auto beta() const { return _beta; }

  /*! \brief Get distance between molecules
   *
   * \param   r_i            Position of molecule i
   * \param   r_j            Position of molecule j
   * \param   edge_lengths   Edge lengths of simulation box
   * \return                 Distance
   */
  inline auto m(const arma::vec &r_i, const arma::vec &r_j,
                const arma::vec &edge_lengths) const {
    return std::get<1>(_m)(r_i, r_j, edge_lengths);
  }

  /*! \brief Get relative position
   *
   * \param   r_i            Position of molecule i
   * \param   r_j            Position of molecule j
   * \param   edge_lengths   Edge lengths of simulation box
   * \return                 Relative position
   */
  inline auto rij(const arma::vec &r_i, const arma::vec &r_j,
                  const arma::vec &edge_lengths) const {
    return std::get<0>(_m)(r_i, r_j, edge_lengths);
  }

  /*! \brief Get edge lengths of simulation box
   *
   * \return      Edge lengths
   */
  inline const auto &edge_lengths() const { return _edge_lengths; }

  /*! \brief Get volume of simulation box
   *
   * \return      Volume
   */
  inline auto V() const { return _V; }

  /*! \brief Add a callback function
   *
   * \param   cb    Callback function to add
   */
  inline void add_parallel_callback(const callback &cb) const {
    _parallel_callbacks.push_back(cb);
  }

  /*! \brief Add a callback function
   *
   * \param   cb    Callback function to add
   */
  inline void add_sequential_callback(const callback &cb) const {
    _sequential_callbacks.push_back(cb);
  }

  /*! \brief Add a callback function to default container
   *
   * \param   cb    Callback function to add
   */
  inline void add_callback(const callback &cb) const {
    add_sequential_callback(cb);
  }

  /*! \brief Add a stopping criterion
   *
   * \param   sc    Stopping criterion
   */
  inline void add_stopping_criterion(const stopping_criterion &sc) const {
    _stopping_criteria.push_back(sc);
  }

  /*! \brief Clear all stopping criteria
   */
  inline void clear_stopping_criteria() const {
    _stopping_criteria.clear();
  }

  /*! \brief Get current step
   *
   * \return    Current step
   */
  inline auto step() const { return _step; }

  /*! \brief Molecular potentials accessor
   *
   * \return    Molecular potentials
   */
  inline const auto &potentials() const { return _potentials; }

  /*! \brief Accessor for trial move
   *
   * \return    Change in x for trial move
   */
  inline const auto &dx() const { return _dx; }

  /*! \brief Index of molecule that has been chosen to move
   *
   * \return      Molecule number of ``chosen'' molecule
   */
  inline auto choice() const { return _choice; }

  /*! \brief Accessor for change in energy that results from the move
   *
   * \return      Change in energy as a result of trial move
   */
  inline const auto &dU() const { return _dU; }

  /*! \brief Accessor for the current system energy
   *
   * \return      System energy
   */
  inline const auto &U() const { return _U; }

  /*! \brief Accessor for random number used to determine acceptance/rejection
   *
   * \return      Epsilon such that epsilon < B results in accepting the move
   */
  inline const auto &eps() const { return _eps; }

  /*! \brief Accessor for whether or not the trial move was accepted
   *
   * \return      Answers: ``was the trial move accepted?''
   */
  inline const auto &accepted() const { return _accepted; }

  /*! \brief Run metropolis simulation
   *
   * \param   nsteps    Number of steps to simulate
   */
  long unsigned simulate(const long unsigned nsteps);

  /*! \brief Set molecular positions
   *
   * \param   new_positions   New position matrix
   */
  inline void set_positions(const arma::mat &new_positions) {
    _positions = new_positions;
    update_U();
  }

  /*! \brief Set molecular position for molecule j
   *
   * \param   new_x   New position
   * \param   j       Index of molecular to change
   */
  inline void set_positions(const arma::vec &new_x, const size_t j) {
    _positions.col(j) = new_x;
    update_U();
  }

  /*! \brief Seed the random number generator
   *
   * \param     sd      Seed
   */
  inline void seed(const unsigned sd) {
    _rng.seed(sd);
  }

  /*! \brief Reset number of steps
   *
   * \param   step    Step number to set
   */
  inline void reset_step(const unsigned long step = 0) {
    _step = step;
  }

private:
  std::vector<molecular_id> _molecular_ids;
  arma::mat _positions;
  arma::vec _edge_lengths;
  double _V;
  std::vector<abstract_potential*> _potentials;
  double _T;
  double _kB;
  double _beta;
  metric _m;
  bc _bc;
  std::default_random_engine _rng;
  std::uniform_real_distribution<double> _eps_dist;
  std::uniform_int_distribution<size_t> _choice_dist;
  trial_move_generator _tmg;
  acc _acc;
  mutable std::vector<callback> _parallel_callbacks;
  mutable std::vector<callback> _sequential_callbacks;
  mutable std::vector<stopping_criterion> _stopping_criteria;
  long unsigned _step;

  arma::vec _dx;
  size_t _choice;
  double _dU;
  double _U;
  double _eps;
  bool _accepted;
};

} // namespace pauth

#endif


/*
 * Copyright 2020 University of Liège
 *
 * 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 "wAdjoint.h"
#include "wProblem.h"
#include "wMedium.h"
#include "wAssign.h"
#include "wFreestream.h"
#include "wWake.h"
#include "wBoundary.h"
#include "wSolver.h"

#include "wMshData.h"
#include "wNode.h"
#include "wElement.h"
#include "wTag.h"
#include "wCache.h"
#include "wMem.h"
#include "wResults.h"
#include "wMshExport.h"
#include "wSparseLu.h"

#include <Eigen/Sparse>

#include <tbb/task_scheduler_init.h>
#include <tbb/tbb.h>
#include <tbb/parallel_do.h>
#include <tbb/spin_mutex.h>

#include <iomanip>

using namespace tbox;
using namespace flow;

#define ANSI_COLOR_YELLOW "\x1b[1;33m"
#define ANSI_COLOR_RESET "\x1b[0m"

/**
 * @brief Initialize the adjoint solver
 * @authors Adrien Crovato
 */
Adjoint::Adjoint(std::shared_ptr<Solver> _sol) : sol(_sol)
{
    // Default parameters
    nthreads = sol->nthreads;
    verbose = sol->verbose;

    // Setup variables (unkwown and residual vectors)
    lambdaL.resize(sol->pbl->msh->nodes.size(), 0.);
    lambdaD.resize(sol->pbl->msh->nodes.size(), 0.);
}

/**
 * @brief Run the linear solver
 *
 * Solve the adjoint steady transonic Full Potential Equation
 * i.e. computes sensitivites for lift and drag cost functions
 * @authors Adrien Crovato
 */
void Adjoint::run()
{
    // Init
    tbb::task_scheduler_init init(nthreads);

    // Display solver parameters
    std::cout << "--- Adjoint solver ---\n"
              << "Number of threads: " << nthreads << "\n"
              << std::endl;
    // Map solution vectors
    Eigen::Map<Eigen::VectorXd> lambdaL_(lambdaL.data(), lambdaL.size()), lambdaD_(lambdaD.data(), lambdaD.size());
    // build SoE
    Eigen::SparseMatrix<double, Eigen::RowMajor> dR(sol->pbl->msh->nodes.size(), sol->pbl->msh->nodes.size());
    buildDR(dR);
    std::vector<double> dummyL(sol->pbl->msh->nodes.size()), dummyD(sol->pbl->msh->nodes.size()); // dummy bs vector
    Eigen::Map<Eigen::VectorXd> dL_(dummyL.data(), dummyL.size()), dD_(dummyD.data(), dummyD.size());
    buildDI(dL_, dD_);

    // Setup linear solver and solve linear SoE dR {lambdaL, lambdaD} = {dL, dD}
    SparseLu lu;
    lu.compute(dR, dL_, lambdaL_);
    lu.compute(dR, dD_, lambdaD_);

    // Check residual
    Eigen::VectorXd rLambdaL = dR * lambdaL_ - dL_;
    Eigen::VectorXd rLambdaD = dR * lambdaD_ - dD_;

    std::cout << std::setw(8) << "L_Iter" << std::setw(15) << "Sens[lambdaL]" << std::setw(15) << "Sens[lambdaD]"
              << std::setw(15) << "Res[lambdaL]" << std::setw(15) << "Res[lambdaD]" << std::endl;
    std::cout << std::fixed << std::setprecision(5);
    std::cout << std::setw(8) << 0 << std::setw(15) << lambdaL_.norm() << std::setw(15) << lambdaD_.norm()
              << std::setw(15) << log10(rLambdaL.norm()) << std::setw(15) << log10(rLambdaD.norm()) << std::endl;

    std::cout << ANSI_COLOR_YELLOW << "Warning: the adjoint solver is experimental and has not been validated!\n"
              << ANSI_COLOR_RESET << std::endl;
}

/**
 * @brief Build the Jacobian matrix
 * @authors Adrien Crovato
 * @todo Refactor, this is heavily copy-pasted from Newton::buildJ
 */
void Adjoint::buildDR(Eigen::SparseMatrix<double, Eigen::RowMajor> &dR)
{
    // Multithread
    tbb::spin_mutex mutex;

    // List of triplets to build Jacobian matrix
    std::deque<Eigen::Triplet<double>> T;

    // Full Potential Equation with upwind bias: analytical derivatives
    auto fluid = sol->pbl->medium;
    tbb::parallel_do(fluid->adjMap.begin(), fluid->adjMap.end(), [&](std::pair<Element *, std::vector<Element *>> p) {
        // Current element
        Element *e = p.first;
        // Upwind element
        Element *eU = p.second[0];

        // Subsonic contribution (drho*grad_phi*grad_psi + rho*grad_phi*grad_psi)
        Eigen::MatrixXd Ae1 = e->buildDK(sol->phi, fluid->rho);
        Eigen::MatrixXd Ae2 = e->buildK(sol->phi, fluid->rho);
        // Supersonic contribution
        double mCOv = 0.95; // solver should be converged, so we set the lowest numerical viscosity
        double muCv = 1.0;
        double mach = fluid->mach.eval(*e, sol->phi, 0);
        if (mach > mCOv)
        {
            // switching function and derivative
            double mu = muCv * (1 - (mCOv * mCOv) / (mach * mach));
            double dmu = 2 * muCv * mCOv * mCOv / (mach * mach * mach);

            // scale 1st and 2nd terms
            Ae1 *= 1 - mu;
            Ae2 *= 1 - mu;

            // 3rd term (mu*drhoU*grad_phi*grad_psi)
            Eigen::MatrixXd Ae3 = mu * fluid->rho.evalD(*eU, sol->phi, 0) * e->buildDs(sol->phi, Fct0C(1.)) *
                                  eU->computeGrad(sol->phi, 0).transpose() * eU->getVCache().getDsf(0);

            // 4th term (mu*rhoU*grad_phi*grad_psi)
            Eigen::MatrixXd Ae4 = mu * fluid->rho.eval(*eU, sol->phi, 0) * e->buildK(sol->phi, Fct0C(1.));

            // 5th term dmu*(rhoU-rho)*grad_phi*grad_psi
            Eigen::MatrixXd Ae5 = dmu * (fluid->rho.eval(*eU, sol->phi, 0) - fluid->rho.eval(*e, sol->phi, 0)) *
                                  e->buildDK(sol->phi, fluid->mach);

            // Assembly (supersonic)
            tbb::spin_mutex::scoped_lock lock(mutex);
            for (size_t ii = 0; ii < e->nodes.size(); ++ii)
            {
                Node *nodi = e->nodes[ii];
                for (size_t jj = 0; jj < e->nodes.size(); ++jj)
                {
                    Node *nodj = e->nodes[jj];
                    T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, Ae4(ii, jj) + Ae5(ii, jj)));
                }
                for (size_t jj = 0; jj < eU->nodes.size(); ++jj)
                {
                    Node *nodj = eU->nodes[jj];
                    T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, Ae3(ii, jj)));
                }
            }
        }
        // Assembly (subsonic)
        tbb::spin_mutex::scoped_lock lock(mutex);
        for (size_t ii = 0; ii < e->nodes.size(); ++ii)
        {
            Node *nodi = e->nodes[ii];
            for (size_t jj = 0; jj < e->nodes.size(); ++jj)
            {
                Node *nodj = e->nodes[jj];
                T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, Ae1(ii, jj) + Ae2(ii, jj)));
            }
        }
    });
    // Apply wake BCs for adjoint problem
    for (auto wake : sol->pbl->wBCs)
    {
        tbb::parallel_do(wake->wEle.begin(), wake->wEle.end(), [&](WakeElement *we) {
            Eigen::MatrixXd Kupup(we->nColUp, we->nRow);
            Eigen::MatrixXd Kuplw(we->nColUp, we->nRow);
            Eigen::MatrixXd Klwup(we->nColLw, we->nRow);
            Eigen::MatrixXd Klwlw(we->nColLw, we->nRow);
            buildWake(we, Kupup, Kuplw, Klwup, Klwlw);
            // Assembly
            tbb::spin_mutex::scoped_lock lock(mutex);
            for (size_t ii = 0; ii < we->nColUp; ++ii)
            {
                Node *nodi = we->volUpE->nodes[ii];
                for (size_t jj = 0; jj < we->nRow; ++jj)
                {
                    Node *nodj = we->surUpE->nodes[jj];
                    T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, 2 * Kupup(ii, jj)));
                    // dR.coeffRef(nodi->row, nodj->row) += 2 * Kupup(ii, jj);
                    nodj = we->surLwE->nodes[jj];
                    T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, Kuplw(ii, jj)));
                    // dR.coeffRef(nodi->row, nodj->row) += Kuplw(ii, jj);
                }
            }
            for (size_t ii = 0; ii < we->nColLw; ++ii)
            {
                Node *nodi = we->volLwE->nodes[ii];
                for (size_t jj = 0; jj < we->nRow; ++jj)
                {
                    Node *nodj = we->surUpE->nodes[jj];
                    // dR.coeffRef(nodi->row, nodj->row) -= 2 * Klwup(ii, jj);
                    T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, -2 * Klwup(ii, jj)));
                    nodj = we->surLwE->nodes[jj];
                    // dR.coeffRef(nodi->row, nodj->row) -= Klwlw(ii, jj);
                    T.push_back(Eigen::Triplet<double>(nodi->row, nodj->row, -Klwlw(ii, jj)));
                }
            }
        });
    }
    // Build Jacobian matrix without BCs
    dR.setFromTriplets(T.begin(), T.end());
    // Apply Dirichlet BCs
    for (auto dBC : sol->pbl->dBCs)
    {
        for (auto nod : dBC->nodes)
        {
            for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(dR, nod->row); it; ++it)
            {
                if (it.row() == it.col())
                    it.valueRef() = 1.;
                else
                    it.valueRef() = 0.;
            }
        }
    }
    for (auto fBC : sol->pbl->fBCs)
    {
        for (auto e : fBC->tag->elems)
        {
            for (auto nod : e->nodes)
            {
                for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(dR, nod->row); it; ++it)
                {
                    if (it.row() == it.col())
                        it.valueRef() = 1.;
                    else
                        it.valueRef() = 0.;
                }
            }
        }
    }
    // Transpose
    dR.transpose();
    // Clean matrix and turn to compressed row format
    dR.prune(0.);
    dR.makeCompressed();

    if (verbose)
        std::cout << "J (" << dR.rows() << "," << dR.cols() << ") nnz=" << dR.nonZeros() << "\n";
}

/**
 * @brief Build the derivatives of the cost functions
 * @authors Adrien Crovato
 * @todo Refactor, to be checked
 */
void Adjoint::buildDI(Eigen::Map<Eigen::VectorXd> &dL, Eigen::Map<Eigen::VectorXd> &dD)
{
    // Multithread
    tbb::spin_mutex mutex;

    // Derivative of lift and drag
    for (auto sur : sol->pbl->bnd)
    {
        tbb::parallel_do(sur->groups[0]->tag->elems.begin(), sur->groups[0]->tag->elems.end(), [&](Element *e) {
            // Build flux factor
            Element *eV = sur->svMap.at(e);
            Eigen::RowVectorXd be(eV->nodes.size());
            buildBoundary(e, eV, be);
            // Compute scaling factor
            Eigen::Vector3d Vi(0., 0., 0.);
            Vi(0) = -sin(sol->pbl->alpha);
            Vi(sol->pbl->nDim - 1) = cos(sol->pbl->alpha);
            double factorL = -1 / sol->pbl->S_ref * sol->pbl->medium->cP.evalD(*eV, sol->phi, 0) * e->normal().dot(Vi);
            Vi(0) = cos(sol->pbl->alpha);
            Vi(sol->pbl->nDim - 1) = sin(sol->pbl->alpha);
            double factorD = -1 / sol->pbl->S_ref * sol->pbl->medium->cP.evalD(*eV, sol->phi, 0) * e->normal().dot(Vi);

            // Assembly
            tbb::spin_mutex::scoped_lock lock(mutex);
            for (size_t ii = 0; ii < eV->nodes.size(); ++ii)
            {
                Node *nodi = eV->nodes[ii];
                dL(nodi->row) += factorL * be(ii);
                dD(nodi->row) += factorD * be(ii);
            }
        });
    }
    // Apply Dirichlet BCs, adjoint solution vanishes at infinity
    for (auto dBC : sol->pbl->dBCs)
    {
        for (auto nod : dBC->nodes)
        {
            dL(nod->row) = 0.;
            dD(nod->row) = 0.;
        }
    }
    for (auto fBC : sol->pbl->fBCs)
    {
        for (auto e : fBC->tag->elems)
        {
            for (auto nod : e->nodes)
            {
                dL(nod->row) = 0.;
                dD(nod->row) = 0.;
            }
        }
    }

    if (verbose)
    {
        std::cout << "dL (" << dL.size() << ")\n";
        std::cout << "dD (" << dD.size() << ")\n";
    }
}

/**
 * @brief Build the boundary contribution for the adjoint equation
 * @authors Adrien Crovato
 * @todo Refactor, to be checked and moved to Boundary::buildAdjNs()
 */
void Adjoint::buildBoundary(Element *&e, Element *&eV, Eigen::RowVectorXd &be)
{
    // Get shape functions
    Eigen::MatrixXd const &volDff = eV->getVCache().getDsf(0);
    // Get Jacobian
    Mem &surMem = e->getVMem();
    Mem &volMem = eV->getVMem();
    // b = V*[inv(J)*dN]
    be = eV->computeGrad(sol->phi, 0).transpose() * volMem.getJinv(0) * volDff * surMem.getVol();
}

/**
 * @brief Build the wake contribution for the adjoint equation
 * @authors Adrien Crovato
 * @todo Refactor, to be checked and moved to WakeElement::buildAdjNK()
 */
void Adjoint::buildWake(WakeElement *&we, Eigen::MatrixXd &Kupup, Eigen::MatrixXd &Kuplw, Eigen::MatrixXd &Klwup,
                        Eigen::MatrixXd &Klwlw)
{
    // Get shape functions and Gauss points
    Cache &surCacheUp = we->surUpE->getVCache();
    Gauss &surGaussUp = surCacheUp.getVGauss();
    Cache &surCacheLw = we->surLwE->getVCache();
    Gauss &surGaussLw = surCacheLw.getVGauss();
    Eigen::MatrixXd const &volUpDff = we->volUpE->getVCache().getDsf(0);
    Eigen::MatrixXd const &volLwDff = we->volLwE->getVCache().getDsf(0);
    // Get Jacobian
    Mem &surMemUp = we->surUpE->getVMem();
    Mem &surMemLw = we->surLwE->getVMem();
    Eigen::MatrixXd const &volUpJ = we->volUpE->getVMem().getJinv(0);
    Eigen::MatrixXd const &volLwJ = we->volLwE->getVMem().getJinv(0);
    // Reset matrices
    Kupup = Eigen::MatrixXd::Zero(we->nColUp, we->nRow);
    Kuplw = Eigen::MatrixXd::Zero(we->nColUp, we->nRow);
    Klwup = Eigen::MatrixXd::Zero(we->nColLw, we->nRow);
    Klwlw = Eigen::MatrixXd::Zero(we->nColLw, we->nRow);

    // unit normal
    Eigen::VectorXd nUp = Eigen::VectorXd::Zero(sol->pbl->nDim);
    Eigen::VectorXd nLw = Eigen::VectorXd::Zero(sol->pbl->nDim);
    nUp(0) = we->surUpE->normal()(0);
    nUp(sol->pbl->nDim - 1) = we->surUpE->normal()(sol->pbl->nDim - 1);
    nLw(0) = we->surLwE->normal()(0);
    nLw(sol->pbl->nDim - 1) = we->surLwE->normal()(sol->pbl->nDim - 1);
    // intermediate vectors
    Eigen::VectorXd nJdNUp(we->nColUp);
    Eigen::VectorXd nJdNLw(we->nColLw);
    Eigen::VectorXd VJdNUp(we->nColUp);
    Eigen::VectorXd VJdNLw(we->nColLw);
    nJdNUp = volUpDff * volUpJ.transpose() * nUp;
    nJdNLw = volLwDff * volLwJ.transpose() * nLw;
    VJdNUp = volUpDff * volUpJ.transpose() * we->volUpE->computeGrad(sol->phi, 0);
    VJdNLw = volLwDff * volLwJ.transpose() * we->volLwE->computeGrad(sol->phi, 0);

    // Build
    /* @todo assemble on contributing nodes for lower surfaces => nRow <- we->surE->nodes.size() */
    for (size_t k = 0; k < surGaussUp.getN(); ++k)
    {
        // N
        Eigen::RowVectorXd Nup = surCacheUp.getSf(k).transpose();
        Eigen::RowVectorXd Nlw = surCacheLw.getSf(k).transpose();
        // K = K + N*VJdN*w*dtm
        Kupup += VJdNUp * Nup * surGaussUp.getW(k) * surMemUp.getDetJ(k);
        Klwup += VJdNLw * Nup * surGaussUp.getW(k) * surMemUp.getDetJ(k);
        // K = K + N*nJdN*w*dtm
        Kuplw += nJdNUp * Nlw * surGaussLw.getW(k) * surMemLw.getDetJ(k);
        Klwlw += nJdNLw * Nlw * surGaussLw.getW(k) * surMemLw.getDetJ(k);
    }
}

/**
 * @brief Write the results
 * @authors Adrien Crovato
 */
void Adjoint::save(int n, std::shared_ptr<MshExport> mshWriter)
{
    // Write files
    std::cout << "Saving files... " << std::endl;
    // setup results
    Results results;
    results.scalars_at_nodes["lambdaL"] = &lambdaL;
    results.scalars_at_nodes["lambdaD"] = &lambdaD;
    // save (all mesh and boundary surface)
    if (n > 0)
    {
        mshWriter->save(sol->pbl->msh->name + "_adjoint_" + std::to_string(n), results);
        for (auto sur : sol->pbl->bnd)
            sur->save(sur->groups[0]->tag->name + "adjoint_" + std::to_string(n), results);
    }
    else
    {
        mshWriter->save(sol->pbl->msh->name + "_adjoint", results);
        for (auto sur : sol->pbl->bnd)
            sur->save(sur->groups[0]->tag->name + "_adjoint", results);
    }
}

void Adjoint::write(std::ostream &out) const
{
    out << "flow::Adjoint"
        << "\n";
}


#include <iostream>
#include <boost/units/unit.hpp>
#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/systems/si.hpp>

using namespace std;
using namespace boost;
using namespace boost::units;
using namespace boost::units::si;

int main()
{
    typedef make_scaled_unit<si::length, scale<10, static_rational<-2>>>::type cm;
    quantity<cm> d(2.0 * si::meter);
    quantity<si::time> t(100.0 * si::seconds);
    quantity<si::velocity> x(d / t);

    cout << d.value() << "cm" << endl;
    cout << t.value() << "sec" << endl;
    cout << x.value() << "m/sec" << endl;
}

/*
200cm
100sec
0.02m/sec
*/


#include <polyfem/SpectralBasis2d.hpp>

#include <polyfem/QuadraticBSpline2d.hpp>
#include <polyfem/QuadQuadrature.hpp>
#include <polyfem/MeshNodes.hpp>

#include <polyfem/LinearSolver.hpp>
#include <polyfem/FEBasis2d.hpp>
#include <polyfem/Types.hpp>

#include <polyfem/Common.hpp>

#include <Eigen/Sparse>

#include <cassert>
#include <iostream>
#include <vector>
#include <array>
#include <map>

namespace polyfem
{
    using namespace Eigen;

    namespace
    {
        void basis(const Eigen::MatrixXd &uv, const int n, const int m, Eigen::MatrixXd &result)
        {
            const int n_pts = int(uv.rows());
            assert(uv.cols() == 2);

            result.resize(n_pts, 1);

            for(int i = 0; i < n_pts; ++i)
                result(i) = sin(n*M_PI*uv(i,0)) * sin(m*M_PI*uv(i,1));
        }

        void derivative(const Eigen::MatrixXd &uv, const int n, const int m, Eigen::MatrixXd &result)
        {
            const int n_pts = int(uv.rows());
            assert(uv.cols() == 2);

            result.resize(n_pts, 2);

            for(int i = 0; i < n_pts; ++i)
            {
                const double u = uv(i,0);
                const double v = uv(i,1);

                result(i,0) = cos(n*M_PI*uv(i,0)) * sin(m*M_PI*uv(i,1));
                result(i,1) = sin(n*M_PI*uv(i,0)) * cos(m*M_PI*uv(i,1));
            }
        }
    }

    int SpectralBasis2d::build_bases(
        const Mesh2D &mesh,
        const int quadrature_order,
        const int order,
        std::vector< ElementBases > &bases,
        std::vector< ElementBases > &gbases,
        std::vector< LocalBoundary > &local_boundary)
    {
        bases.resize(1);
        ElementBases &b = bases.front();
        b.has_parameterization = false;

        const int n_bases = order * order;

        b.bases.resize(n_bases);
        b.set_quadrature([quadrature_order](Quadrature &quad){
            QuadQuadrature quad_quadrature;
            quad_quadrature.get_quadrature(quadrature_order, quad);
        });

        for (int i = 0; i < order; ++i) {
            for (int j = 0; j < order; ++j) {
                const int global_index = order*i + j;
                assert(global_index < n_bases);

                b.bases[global_index].init(-3, global_index, j, Eigen::MatrixXd::Zero(1, 2));
                b.bases[global_index].set_basis([i, j](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { basis(uv, i, j, val); });
                b.bases[global_index].set_grad([i, j](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { derivative(uv, i, j, val); });
            }
        }

        // gbases.resize(1);
        // ElementBases &gb = bases.front();
        // gb.bases.resize(n_bases);
        // gb.set_quadrature([quadrature_order](Quadrature &quad){
        //     QuadQuadrature quad_quadrature;
        //     quad_quadrature.get_quadrature(quadrature_order, quad);
        // });
        // b.has_parameterization = false;

        // for (int j = 0; j < n_bases; ++j) {
        //     const int global_index = j;

        //     gb.bases[j].init(global_index, j, Eigen::Vector2d(0,0));
        //     gb.bases[j].set_basis([j](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { basis(uv, j, val); });
        //     gb.bases[j].set_grad([j](const Eigen::MatrixXd &uv, Eigen::MatrixXd &val) { derivative(uv, j, val); });
        // }

        return n_bases;
    }

}


/******************************************************************************

  Dual-point generalized Hough Transform for 2D object recognition.

  Copyright (c) 2013
  Dzmitry Hlindzich <hlindzich@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef DPG_HOUGH_TRANSFORM_HPP_55B0255B_E6C8_4302_9114_D1B684CD3419_
#define DPG_HOUGH_TRANSFORM_HPP_55B0255B_E6C8_4302_9114_D1B684CD3419_

#include <utility>
#include <vector>
#include <list>
#include <set>
#include <cmath>
#include <algorithm>
#include <complex>
#include <boost/noncopyable.hpp>
#include <boost/assert.hpp>
#include <boost/math/constants/constants.hpp>

#include "bo/config.hpp"
#include "bo/core/vector.hpp"
#include "bo/math/blas_extensions.hpp"
#include "bo/core/raw_image_2d.hpp"
#include "bo/math/topology.hpp"
#include "bo/math/blas_extensions.hpp"
#include "bo/surfaces/convex_hull_3d.hpp"

namespace bo {
namespace recognition {

namespace detail {

// A 4-dimensional hyperplane defined by a point located
// on this plane and the plane's normal vector.
template <typename RealType>
class Hyperplane4D
{
public:
    typedef Vector<RealType, 4> Point4D;

    Hyperplane4D(const Point4D &point, const Point4D &normal)
        : point_(point), kEpsilon(0.0001)
    {
        Point4D n = normal / normal.euclidean_norm();
        normal_ = n;

        // Decomposition matrix (inverse of the base one).
        m_ = math::matrix<RealType>(4, 4);
        m_(0, 0) =  n[3]; m_(0, 1) =  n[2]; m_(0, 2) = -n[1]; m_(0, 3) = -n[0];
        m_(1, 0) = -n[2]; m_(1, 1) =  n[3]; m_(1, 2) =  n[0]; m_(1, 3) = -n[1];
        m_(2, 0) =  n[1]; m_(2, 1) = -n[0]; m_(2, 2) =  n[3]; m_(2, 3) = -n[2];
        m_(3, 0) =  n[0]; m_(3, 1) =  n[1]; m_(3, 2) =  n[2]; m_(3, 3) =  n[3];
    }

    inline
    const Point4D& point() const
    { return point_; }

    inline
    const Point4D& normal() const
    { return normal_; }

    // Computes intersection of a line defined by two 4-dimensional points
    // with the hyperplane. Considers the intersection as a linear coordinate 't'
    // such that the intersection point is calculated as P = q1 + t * (q2 - q1).
    // If the line intersects with the hyperplane, returns true and updates the
    // function parameter 't'. Otherwise returns false.
    bool intersect(const Point4D& q1, const Point4D& q2, RealType& t)
    {
        Point4D q1p = point_ - q1;
        Point4D e = q2 - q1;
        RealType dot1 = q1p * normal_;

        RealType e_norm = e.euclidean_norm();
        // This case is needed for continuity, when two very close points
        // both belong to the hyperplane.
        if (e_norm < kEpsilon)
        {
            if (dot1 < kEpsilon)
            {
                t = RealType(0);
                return true;
            }
            else
            {
                return false;
            }
        }

        RealType dot2 = e * normal_;

        if (std::abs(dot2 / e_norm) > kEpsilon)
        {
            // If the line is not parallel to the hyperplane.
            t = dot1 / dot2;
            return true;
        }
        else
        {
            return false;
        }
    }

    // Computes the decomposition of the point in the hyperplane's
    // coordinate system defined by the decomposition matrix.
    Point4D decompose(const Point4D& p)
    {
        Point4D q = p - point_;

        math::matrix<RealType> v(4, 1);
        v(0, 0) = q[0];
        v(1, 0) = q[1];
        v(2, 0) = q[2];
        v(3, 0) = q[3];

        // Compute decomposition in the projection basis.
        v = math::prod(m_, v);

        return Point4D(v(0, 0), v(1, 0), v(2, 0), v(3, 0));
    }

private:

    Point4D point_;
    Point4D normal_;

    // Hyperplane's decomposition matrix.
    math::matrix<RealType> m_;

    const RealType kEpsilon;
};

template <typename RealType>
class Space
{
public:
    typedef Vector<RealType, 4> Point4D;
    typedef std::vector<Point4D> Points4D;
    typedef Vector<std::size_t, 4> Size4D;
    typedef std::pair<Point4D, Point4D> Box4D;
    typedef bo::math::OrthotopeTopology<RealType, 4> Geometry;
    typedef detail::Hyperplane4D<RealType> Hyperplane;

    // A line in 4D is defined by any point located
    // on this line and its directional vector.
    struct Line4D
    {
        Line4D()
            : point(Point4D(0)), direction(Point4D(0))
        { }

        Line4D(const Point4D &_point, const Point4D &_direction)
            : point(_point), direction(_direction)
        { }

        Point4D point;
        Point4D direction;
    };

    typedef Vector<RealType, 2> SegmentCoordinates;
    typedef std::vector<Space> Spaces;

    // A segment in 4D is modelled by a 4D line and the
    // coordinates of two points on this line
    // relatively to the line's direction vector.
    typedef std::pair<Line4D, SegmentCoordinates> Segment4D;

    Space(const Box4D &box = Box4D(Point4D(0, 0, 0, 0), Point4D(0, 0, 0, 0)),
          const Size4D &divisions_per_dimension = Size4D(2, 2, 2, 2),
          std::size_t max_resolution_level = 1,
          std::size_t cell_resolution_increment = 1,
          std::size_t resolution_level = 0):
        box_(box),  divisions_per_dimension_(divisions_per_dimension),
        max_resolution_level_(max_resolution_level), resolution_level_(resolution_level),
        votes_(0)
    {
        BOOST_ASSERT(resolution_level_ <= max_resolution_level_);

        // Adjust the cell resolution increment.
        if (resolution_level_ + cell_resolution_increment > max_resolution_level_)
        {
            cell_resolution_increment_ = max_resolution_level_ - resolution_level_;
        }
        else
        {
            cell_resolution_increment_ =  cell_resolution_increment;
        }

        // Compute the size of cells used for vote calculation.
        for (std::size_t i = 0; i < 4; ++i)
        {
            std::size_t cells_in_dimension = static_cast<std::size_t>(
                std::pow(RealType(divisions_per_dimension_[i]),
                         RealType(max_resolution_level_) - RealType(resolution_level_)));

            cell_size_[i] = (box_.second[i] - box_.first[i]) /  cells_in_dimension;
        }

        min_cell_volume_ = std::pow((cell_size_[0] + cell_size_[1] + cell_size_[2] +
                cell_size_[3]) / 4, 3);

        // Compute the number of probabilistic elements used for the subdivision policy.
        prob_element_count_ = 1;
        for (std::size_t i = 0; i < 4; ++i)
        {
            std::size_t prob_elements_in_dimension = static_cast<std::size_t>(
                std::pow(RealType(divisions_per_dimension_[i]), RealType(cell_resolution_increment_)));

            prob_element_count_ *= prob_elements_in_dimension;
        }
    }

    // Subdivides the space.
    void subdivide()
    {
        subspaces_.clear();

        std::size_t subdivision_count_ = divisions_per_dimension_[0] * divisions_per_dimension_[1] *
                                         divisions_per_dimension_[2] * divisions_per_dimension_[3];

        subspaces_.reserve(subdivision_count_);

        Point4D sizes4d = box_.second - box_.first;
        Point4D steps4d = Point4D(sizes4d[0] / RealType(divisions_per_dimension_[0]),
                                  sizes4d[1] / RealType(divisions_per_dimension_[1]),
                                  sizes4d[2] / RealType(divisions_per_dimension_[2]),
                                  sizes4d[3] / RealType(divisions_per_dimension_[3]));

        for (std::size_t d0 = 0; d0 < divisions_per_dimension_[0]; ++d0)
            for (std::size_t d1 = 0; d1 < divisions_per_dimension_[1]; ++d1)
                for (std::size_t d2 = 0; d2 < divisions_per_dimension_[2]; ++d2)
                    for (std::size_t d3 = 0; d3 < divisions_per_dimension_[3]; ++d3)
                    {
                        // Create a subspace.
                        Point4D translation(d0 * steps4d[0], d1 * steps4d[1], d2 * steps4d[2],
                                d3 * steps4d[3]);
                        Box4D b(box_.first + translation, box_.first + translation + steps4d);
                        Space s(b, divisions_per_dimension_, max_resolution_level_,
                                cell_resolution_increment_, resolution_level_ + 1);

                        subspaces_.push_back(s);
                    }

    }

    inline Spaces& get_subspaces()
    {
        return subspaces_;
    }

    inline const Spaces& get_subspaces() const
    {
         return subspaces_;
    }

    inline const Box4D& get_bounding_box() const
    {
        return box_;
    }

    inline RealType get_votes() const
    {
        return votes_;
    }

    inline std::size_t get_prob_element_count() const
    {
        return prob_element_count_;
    }

    void reset_votes()
    {
        votes_ = 0;
    }

    inline std::size_t get_resolution_level() const
    {
        return resolution_level_;
    }

    inline std::size_t get_max_resolution_level() const
    {
        return max_resolution_level_;
    }

    inline Point4D get_mass_center() const
    {
        return (box_.first + box_.second) / 2;
    }

    void vote(Segment4D segment)
    {
        // The number of votes that the space receives in the result of the intersection
        // equals to the lenght of the resulting segment.
        // Compute the segment.
        Point4D s = segment.first.direction * (segment.second[1] - segment.second[0]);

        // Increase the votes.
        // Continuous. Attention: has no effect if the intersection is in one point.
        votes_ += s.euclidean_norm();
    }

    void vote_unit(const Segment4D &segment)
    {
        // Binary.
        if (segment.second[0] > -std::numeric_limits<RealType>::max() &&
            segment.second[0] < std::numeric_limits<RealType>::max())
        {
            votes_ += 1;
        }
    }

    // Increases the number of votes on the number of intersection of the given segment
    // with the grid defined by the space cell size.
    void vote_descrete(const Segment4D &segment)
    {
        // We will accumulate the coordinates of intersections of the given segment
        // with the grid of the given cell size.
        std::set<RealType> intersections;

        // Find coordinates of the intersection of the line with the space.
        Segment4D base_seg = intersect(segment.first);

        RealType t1_base = base_seg.second[0];
        RealType t2_base = base_seg.second[1];
        // Order.
        order(t1_base, t2_base);

        RealType t1 = segment.second[0];
        RealType t2 = segment.second[1];
        // Order.
        order(t1, t2);

        // Segment direction.
        Point4D v = segment.first.direction;

        // For each dimension define the coordinate step beetween two cells (dimension levels).
        for (std::size_t d = 0; d < 4; ++d)
        {
            if (v[d] != 0)
            // If it is zero, the segment does not intersect the levels of this dimension and
            // we can skip further analysis.
            {
                RealType dt = std::abs(cell_size_[d] / v[d]);

                // Accumulate all intersections of this dimension situated between the begin
                // and the end of the segment. TODO: optimize it!
                for (RealType t = t1_base + dt; t < t2_base; t += dt)
                {
                    if (t > t1 && t < t2)
                        intersections.insert(t);
                }
            }
        }

        votes_ += intersections.size() + 1;
    }

    // Increases the votes on the number equal to the taxicab norm of the
    // corresponding inner segment relatively to the space grid.
    void vote_taxicab(const Segment4D &segment)
    {
        // If the segment is not zero.
        if (segment.second[0] == segment.second[1])
            return;

        std::size_t taxicab_dst = 0;

        // Local coordinate origin.
        Point4D bot = box_.first;
        Point4D top = box_.second;

        // The end points of the segment.
        Point4D p1 = segment.first.point + segment.first.direction * segment.second[0];
        Point4D p2 = segment.first.point + segment.first.direction * segment.second[1];

        // Segment direction.
        Point4D v = segment.first.direction;

        // For each dimension define the coordinate step beetween two cells (dimension levels).
        for (std::size_t d = 0; d < 4; ++d)
        {
            if (v[d] != 0)
            // If it is zero, the segment does not intersect the levels of this dimension and
            // we can skip further analysis.
            {
                RealType proj1 = p1[d];
                RealType proj2 = p2[d];
                order(proj1, proj2);

                order(bot[d], top[d]);

                cut(proj1, bot[d], top[d]);
                cut(proj2, bot[d], top[d]);

                BOOST_ASSERT(proj1 >= bot[d] && proj2 >= bot[d]);

                RealType block1 = std::floor((proj1 - bot[d]) / cell_size_[d]);
                RealType block2 =  std::ceil((proj2 - bot[d]) / cell_size_[d]);

                taxicab_dst += static_cast<std::size_t>(block2 - block1);
            }
        }

        votes_ += taxicab_dst;
    }

    // Increases the votes on the number equal to the maximum (uniform) norm of the
    // inner segment relatively to the space grid.
    void vote_maxnorm(const Segment4D &segment)
    {
        // If the segment is not zero.
        if (segment.second[0] == segment.second[1])
            return;

        std::size_t maximum_norm = 0;

        // Local coordinate origin.
        Point4D bot = box_.first;
        Point4D top = box_.second;

        // The end points of the segment.
        Point4D p1 = segment.first.point + segment.first.direction * segment.second[0];
        Point4D p2 = segment.first.point + segment.first.direction * segment.second[1];

        // Segment direction.
        Point4D v = segment.first.direction;

        // For each dimension define the coordinate step beetween two cells (dimension levels).
        for (std::size_t d = 0; d < 4; ++d)
        {
            if (v[d] != 0)
            // If it is zero, the segment does not intersect the levels of this dimension and
            // we can skip further analysis.
            {
                RealType proj1 = p1[d];
                RealType proj2 = p2[d];
                order(proj1, proj2);

                order(bot[d], top[d]);

                cut(proj1, bot[d], top[d]);
                cut(proj2, bot[d], top[d]);

                BOOST_ASSERT(proj1 >= bot[d] && proj2 >= bot[d]);

                RealType block1 = std::floor((proj1 - bot[d]) / cell_size_[d]);
                RealType block2 =  std::ceil((proj2 - bot[d]) / cell_size_[d]);

                std::size_t dst = static_cast<std::size_t>(block2 - block1);

                if (maximum_norm < dst)
                    maximum_norm = dst;
            }
        }

        votes_ += maximum_norm;
    }

    // Returns the result of intersection of the given segment with the space.
    Segment4D intersect(Segment4D segment)
    {
        // Cut the segment in each of four dimensions.
        for (std::size_t d = 0; d < 4; ++d)
            cut_segment(segment, d, box_.first[d], box_.second[d]);

        return segment;
    }

    // Returns the result of intersection (segment) of the given 4D line with the space.
    Segment4D intersect(Line4D line)
    {
        // Create the "infinite" segment coordinates that models the whole line.
        RealType mmax = std::numeric_limits<RealType>::max();
        SegmentCoordinates coord(-mmax, mmax);

        // Create the "infinite" segment.
        Segment4D infinite_seg(line, coord);

        return intersect(infinite_seg);
    }

    // Returns the points that define the polyhedron of the hyperplane-hyperrectangle
    // intersection.
    Points4D intersect(Hyperplane plane)
    {
        Points4D vertices;

        Point4D d = box_.second - box_.first;

        typename Geometry::Edges edges = Geometry::edges();

        for (typename Geometry::Edges::const_iterator it = edges.begin();
             it != edges.end(); ++it)
        {
            typename Geometry::Point e1 = it->first;
            typename Geometry::Point e2 = it->second;

            // Calculate the adjacent vertices of the box edges.
            Point4D q1 = box_.first + Point4D(e1[0] * d[0], e1[1] * d[1],
                                              e1[2] * d[2], e1[3] * d[3]);
            Point4D q2 = box_.first + Point4D(e2[0] * d[0], e2[1] * d[1],
                                              e2[2] * d[2], e2[3] * d[3]);

            // Calculate intersection of the edge with the plane.
            RealType t;
            if (plane.intersect(q1, q2, t))
            {
                // Add the intersection point if it belongs to the edge.
                if (t >= 0 && t <= 1)
                {
                    vertices.push_back(q1 + t * (q2 - q1));
                }
            }
        }

        return vertices;
    }

    RealType intersection_volume(Hyperplane plane)
    {
        RealType kEpsilon(0.001);

        // The points of intersection with the plane in 4D.
        Points4D vertices4 = intersect(plane);

        // Container for projections.
        typedef surfaces::IncrementalConvexHull3D<RealType> Hull;
        typename Hull::Points3D vertices3;

        // Project the vertices into 3D.
        for (typename Points4D::const_iterator it = vertices4.begin();
             it != vertices4.end(); ++it)
        {
            Point4D p = plane.decompose(*it);

            // The vertex must lie in 3D!
            BOOST_ASSERT(std::abs(p[3]) < kEpsilon);
            BO_UNUSED(kEpsilon);

            // Projection.
            typename Hull::Point3D pr(p[0], p[1], p[2]);

            vertices3.push_back(pr);
        }

        if(vertices3.size() > 3)
        {
            // Compute the convex hull and its volume.
            Hull hull3d(vertices3);
            return hull3d.get_volume();
        }
        else
        {
            return 0;
        }
    }

    // The number of votes that the space receives is equal to the volume
    // of the resulting hyperrectangle-hyperplane intersection.
    void vote(Hyperplane plane)
    {
        RealType volume = intersection_volume(plane);

        votes_ += volume;
    }

    // The number of votes that the space receives is equal to the number
    // of the cells that the hyperrectangle-hyperplane intersection "covers".
    void vote_descrete(Hyperplane plane)
    {
        RealType volume = intersection_volume(plane);

        RealType vote = volume / min_cell_volume_;

        votes_ += std::floor(vote) + 1;
    }

    void vote_unit(Hyperplane plane)
    {
        if (intersect(plane).size() > 0)
            votes_ += 1;
    }

private:
    Spaces subspaces_;
    Box4D box_;
    Size4D divisions_per_dimension_;
    std::size_t max_resolution_level_;
    std::size_t cell_resolution_increment_;
    std::size_t resolution_level_;
    RealType votes_;
    Point4D cell_size_;
    RealType min_cell_volume_;
    std::size_t prob_element_count_;

    // Cuts the segment in the given dimension according to two given levels.
    void cut_segment(Segment4D &seg, std::size_t dimension, RealType level1, RealType level2)
    {
        // Sort the levels: level1 <= level2.
        order(level1, level2);

        // The origin point and the direction.
        Point4D p = seg.first.point;
        Point4D v = seg.first.direction;

        // If the segment is parallel to the levels of this dimension.
        if (v[dimension] == 0)
        {
             // If the origin is outside the levels, return infinite point.
            if (p[dimension] > level2 || p[dimension] < level1)
                seg.second[0] = seg.second[1] = std::numeric_limits<RealType>::max();

            // Otherwise return the same segment.
            return;
        }

        // Define the coordinates of the line instersection with the dimension levels.
        RealType t1 = (level1 - p[dimension]) / v[dimension];
        RealType t2 = (level2 - p[dimension]) / v[dimension];

        // Sort the coordinates: t1 <= t2.
        order(t1, t2);

        // If the segments is outside the levels (less), move it in the negative
        // infinite point.
        if (seg.second[0] < t1 && seg.second[1] < t1)
        {
            seg.second[0] = seg.second[1] = -std::numeric_limits<RealType>::max();
            return;
        }

        // If the segments is outside the levels (less), move it in the positive
        // infinite point.
        if (seg.second[0] > t2 && seg.second[1] > t2)
        {
            seg.second[0] = seg.second[1] = std::numeric_limits<RealType>::max();
            return;
        }

        // Cut the segment.
        for (int i = 0; i < 2; ++i)
        {
            if (seg.second[i] < t1)
                seg.second[i] = t1;

            if (seg.second[i] > t2)
                seg.second[i] = t2;
        }

    }

    inline void order(RealType &a, RealType &b)
    {
        if (a > b) std::swap(a, b);
    }

    inline void cut(RealType &x, RealType a, RealType b)
    {
        if (x < a)
            x = a;
        if (x > b)
            x = b;
    }
};

template <typename RealType>
class SubdivisionPolicy
{
public:
    typedef Space<RealType> Space4D;

    // Returns the minimal number of spaces (at the current resolution level) from
    // the beginning of the given sorted collection such that occurrence of the
    // subspaces (at the cell resolution level) with maximal number of votes is
    // not less then the given probability p. Attention: the input collection of
    // spaces must be sorted in descending order!
    static std::size_t probabilistic(const typename Space4D::Spaces &spaces, RealType p)
    {
        typename Space4D::Spaces subcollection1;
        typename Space4D::Spaces subcollection2 = spaces;

        std::size_t n = 0;

        // Find the minimal number of spaces that satisfy the probability constraint.
        while (n < spaces.size() &&
               p_max_value_in_subcollection(subcollection1, subcollection2) < p)
        {
            typename Space<RealType>::Spaces::iterator it = subcollection2.begin();

            subcollection1.push_back(*it);
            subcollection2.erase(it);
            n = subcollection1.size();
        }

        return n;
    }

    // Computes the probability of the event that the maximal value of subcollection1 is greater
    // than the maximal value from subcollection2.
    static RealType p_max_value_in_subcollection(const typename Space4D::Spaces &subcollection1,
                                                 const typename Space4D::Spaces &subcollection2)
    {
        // Compute the maximal vote in subcollection1.
        std::size_t max_votes = 0;
        for (typename Space4D::Spaces::const_iterator it = subcollection1.begin();
             it != subcollection1.end(); ++it)
        {
            std::size_t votes = std::size_t(it->get_votes());
            if (max_votes < votes)
            {
                max_votes = votes;
            }
        }

        RealType p = 0;

        // Compute the probability.
        for (std::size_t t = 1; t <= max_votes; ++t)
        {
            RealType f1 = F_joint(subcollection1, t);
            RealType f2 = F_joint(subcollection1, t - 1);
            RealType f3 = F_joint(subcollection2, t - 1);

            p += (f1 - f2) * f3;
        }

        return p;
    }

    // Joint distribution function for spaces.
    static RealType F_joint(const typename Space4D::Spaces &spaces, std::size_t x)
    {
        if (spaces.size() == 0) return 0;

        RealType f = 1;

        for (typename Space4D::Spaces::const_iterator it = spaces.begin();
             it != spaces.end(); ++it)
        {
            f *= F(std::size_t(it->get_votes()), it->get_prob_element_count(), x);
        }

        return f;
    }

    // Distribution function.
    static RealType F(std::size_t k, std::size_t n, std::size_t x)
    {
        // Some cases of small values (k, n) are implemented explicitly
        // avoiding the Gumbel-based approximation in order to increase
        // precision.
        if (k == 0)
        {
            return RealType(1);
        }
        if (n == 1)
        {
            return (x >= k) ? RealType(1) : RealType(0);
        }

        return std::exp(-std::exp((mu(k, n) - x) / beta(k, n)));
    }

    // Mean.
    static RealType E(std::size_t k, std::size_t n)
    {
        // Euler–Mascheroni constant.
        const RealType gamma = RealType(0.5772);

        return mu(k, n) + gamma * beta(k, n);
    }

    static RealType mu(std::size_t k, std::size_t n)
    {
        return RealType(1.16) * k * std::pow(RealType(n), RealType(-2) / 3) + 1;
    }

    static RealType beta(std::size_t k, std::size_t n)
    {
        return RealType(0.4) * k * std::pow(RealType(n), RealType(-4) / 5) + RealType(0.32);
    }
};

// Compares two spaces using an approximation of the Gumbold means.
template <typename RealType>
bool operator < (const Space<RealType> &s1, const Space<RealType> &s2)
{
    RealType mean1 = SubdivisionPolicy<RealType>::E(std::size_t(s1.get_votes()),
                                                    s1.get_prob_element_count());
    RealType mean2 = SubdivisionPolicy<RealType>::E(std::size_t(s2.get_votes()),
                                                    s2.get_prob_element_count());
    return
        (mean1 < mean2) ? true : false;
}

} // namespace detail

template <typename RealType>
class DualPointGHT: public boost::noncopyable
{
public:
    typedef DualPointGHT<RealType> this_type;
    typedef Vector<RealType, 2> Point2D;
    typedef std::vector<Point2D> Points2D;
    typedef std::pair<Point2D, Point2D> Reference;
    typedef std::pair<Point2D, Point2D> SearchArea;
    typedef std::pair<Reference, RealType> ReferenceVote;
    typedef std::vector<ReferenceVote> ReferenceVotes;
    // Feature is a model point and a tangent vector.
    typedef std::pair<Point2D, Point2D> Feature;
    typedef std::vector<Feature> Features;
    typedef std::pair<RealType, RealType> ATableElement;
    typedef std::list<ATableElement> ATableRow;
    typedef std::vector<ATableRow> ATable;
    typedef detail::Space<RealType> Space4D;
    typedef detail::SubdivisionPolicy<RealType> SubPolicy;

    DualPointGHT(const Features &model_features, const Reference &model_reference,
                 RealType tangent_accuracy = RealType(0.005)):
    model_reference_(model_reference), tangent_accuracy_(tangent_accuracy),
    pi_(boost::math::constants::pi<RealType>())
    {
        encode(model_features);

        model_base_ = (model_reference_.second - model_reference_.first).euclidean_norm();
    }

    void project_detected_lines(const Features &object_features, const Point2D &scaling_range,
                                bo::RawImage2D<RealType> &image1, bo::RawImage2D<RealType> &image2)
    {
        for (typename Features::const_iterator it = object_features.begin();
             it != object_features.end(); ++it)
        {
            // Reconstruct all the 4D lines from the alpha-table relatively to the current feature.
            for (std::size_t index = 0; index < atable_.size(); ++index)
            {
                // Probable angle between the directional vector and the tangent.
                RealType gamma = atable_gamma(index);

                // Find all corresponding directional vectors v2 defined by the
                // (alpha, beta) angles of the current table row.
                for (typename ATableRow::const_iterator abit = atable_.at(index).begin();
                     abit !=  atable_.at(index).end(); ++abit)
                {
                    typename Space4D::Line4D line4 = line4_from_feature_and_atable_element(
                                *it, gamma, *abit);

                    // Create two segments on this line that correspond to the given scaling range.
                    Point2D v1(line4.direction[0], line4.direction[1]);
                    Point2D v2(line4.direction[2], line4.direction[3]);
                    RealType vnorm = (v2 - v1).euclidean_norm();
                    Point2D segment_coords = scaling_range * model_base_ / vnorm;

                    typename Space4D::Segment4D segment1(line4,  segment_coords);
                    typename Space4D::Segment4D segment2(line4, -segment_coords);

                    project_segment(segment1, image1, image2);
                    project_segment(segment2, image1, image2);
                }
            }
        }
    }

    void project_segment(const typename Space4D::Segment4D &segment,
                         bo::RawImage2D<RealType> &image1, bo::RawImage2D<RealType> &image2)
    {
        const RealType delta_t = RealType(0.01);

        RealType t1 = segment.second[0];
        RealType t2 = segment.second[1];

        typename Space4D::Point4D p = segment.first.point;
        typename Space4D::Point4D v = segment.first.direction;

        if (t1 > t2)
            std::swap(t1, t2);

        Vector<int, 4> tmp(0, 0, 0, 0);

        for (RealType t = t1; t < t2; t += delta_t)
        {
            typename Space4D::Point4D xt = p + v * t;

            Vector<int, 4> rounded((int)xt[0], (int)xt[1], (int)xt[2], (int)xt[3]);

            if (rounded != tmp)
            {
                tmp = rounded;

                if (rounded[0] >= 0 && rounded[0] < image1.width() &&
                    rounded[1] >= 0 && rounded[1] < image1.height())
                {
                    image1(rounded[0], rounded[1]) += 1;
                }

                if (rounded[2] >= 0 && rounded[2] < image2.width() &&
                    rounded[3] >= 0 && rounded[3] < image2.height())
                {
                    image2(rounded[2], rounded[3]) += 1;
                }
            }
        }
    }

    // Detects the references that define probable poses of the model within the
    // given features.
    ReferenceVotes fast_detect(const Features &object_features, RealType probability,
                               typename Space4D::Size4D divisions_per_dimension,
                               std::size_t maximal_resolution_level,
                               std::size_t cell_resolution_increment,
                               SearchArea reference_box1,
                               SearchArea reference_box2,
                               Point2D scaling_range = Point2D(0.95f, 1.05f))
    {
        ReferenceVotes ref_votes;

        // Create the root space object.
        typename Space4D::Point4D p1(reference_box1.first[0], reference_box1.first[1],
                                  reference_box2.first[0], reference_box2.first[1]);
        typename Space4D::Point4D p2(reference_box1.second[0], reference_box1.second[1],
                                  reference_box2.second[0], reference_box2.second[1]);

        Space4D s (typename Space4D::Box4D(p1, p2), divisions_per_dimension,
                   maximal_resolution_level, cell_resolution_increment, 0);

        // In the case if the scaling is incorrect.
        normalize_scaling_range(scaling_range);

        // Hierarchical search for the vote peak in the space.
        process_space(s, object_features, scaling_range, probability);

        // Get all subspaces from the last resolution level;
        typename Space4D::Spaces leafs;
        get_resolution_level(s, leafs, maximal_resolution_level);

        // Sort in descending order.
        std::sort(leafs.rbegin(), leafs.rend());

        // Extract the references.
        for (typename Space4D::Spaces::const_iterator it = leafs.begin();
             it != leafs.end(); ++it)
        {
            // Attention: the space is approximated by its mass center!
            typename Space4D::Point4D c = it->get_mass_center();

            Reference ref(Point2D(c[0], c[1]), Point2D(c[2], c[3]));
            ReferenceVote rv(ref, it->get_votes());
            ref_votes.push_back(rv);
        }

        return ref_votes;
    }

    // Detects the references that define probable poses of the model within the
    // given features.
    ReferenceVotes cross_level_detect(const Features &object_features, RealType probability,
                                      typename Space4D::Size4D divisions_per_dimension,
                                      std::size_t maximal_resolution_level,
                                      std::size_t cell_resolution_increment,
                                      SearchArea reference_box1,
                                      SearchArea reference_box2,
                                      Point2D scaling_range = Point2D(0.95f, 1.05f))
    {
        ReferenceVotes ref_votes;

        // Create the root space object.
        typename Space4D::Point4D p1(reference_box1.first[0], reference_box1.first[1],
                                  reference_box2.first[0], reference_box2.first[1]);
        typename Space4D::Point4D p2(reference_box1.second[0], reference_box1.second[1],
                                  reference_box2.second[0], reference_box2.second[1]);

        Space4D s (typename Space4D::Box4D(p1, p2), divisions_per_dimension,
                   maximal_resolution_level, cell_resolution_increment, 0);

        // In the case if the scaling is incorrect.
        normalize_scaling_range(scaling_range);

        std::size_t divisions = divisions_per_dimension[0] * divisions_per_dimension[1] *
                divisions_per_dimension[2] * divisions_per_dimension[3];

        // Initialize temporary collections of spaces.
        typename Space4D::Spaces in, out;
        in.push_back(s);

        for (std::size_t level = 0; level < maximal_resolution_level; ++level)
        {
            out.clear();
            out.reserve(in.size() * divisions);

            // Compute the votes for the level subdivision spaces.
            for (typename Space4D::Spaces::iterator it = in.begin(); it != in.end(); ++it)
            {
                subdivide_with_votes(*it, object_features, scaling_range);
                // Insert all subspaces into the output collection.
                out.insert(out.end(), it->get_subspaces().begin(), it->get_subspaces().end());
            }

            // Sort the subspaces in descending order.
            std::sort(out.rbegin(), out.rend());

            // Find minimal number of subspaces that satisfy the probability constrains.
            std::size_t n;
            if (level == maximal_resolution_level - 1)
            {
                n = out.size();
            }
            else
            {
                n = SubPolicy::probabilistic(out, probability);
            }

            // Update the input collection with the best subspaces.
            in.clear();
            in.insert(in.end(), out.begin(), out.begin() + n);
        }

        // Extract the references.
        for (typename Space4D::Spaces::const_iterator it = in.begin(); it != in.end(); ++it)
        {
            // Attention: the space is approximated by its mass center!
            typename Space4D::Point4D c = it->get_mass_center();

            Reference ref(Point2D(c[0], c[1]), Point2D(c[2], c[3]));
            ReferenceVote rv(ref, it->get_votes());
            ref_votes.push_back(rv);
        }

        return ref_votes;
    }

    // Reconstructs the points of the model that has the pose defined by the given reference.
    Points2D reconstruct(const Reference &reference_points)
    {
        Points2D points;

        // Reconstruct the model points from all alpha and beta angles and
        // the given two reference points.
        for (typename DualPointGHT<RealType>::ATable::const_iterator ait = atable_.begin();
             ait != atable_.end(); ++ait)
            for (typename DualPointGHT<RealType>::ATableRow::const_iterator it = ait->begin();
                 it != ait->end(); ++it)
            {
                points.push_back(find_intersection(reference_points, *it));
            }

        return points;
    }

private:
    Reference model_reference_;
    RealType model_base_;
    RealType tangent_accuracy_;
    ATable atable_;
    RealType pi_;

    // Row index in the alpha-table for the given tangent angle.
    inline std::size_t atable_index(RealType gamma)
    {
        return static_cast<std::size_t>((gamma + pi_) / (2 * pi_) * (atable_.size() - 1));
    }

    // Approximate gamma angle that corresponds to the alpha-table row index.
    inline RealType atable_gamma(std::size_t index)
    {
        return index * 2 * pi_ / (atable_.size() - 1) - pi_;
    }

    // Rotates the given vector in a radians.
    inline Point2D rotate(const Point2D &v, RealType a) const
    {
        RealType cosa = std::cos(a);
        RealType sina = std::sin(a);

        return Point2D(cosa * v[0] - sina * v[1], sina * v[0] + cosa * v[1]);
    }

    // Computes the "positive" normal to the given vector.
    inline Point2D normal(const Point2D &v)
    {
        return rotate(v, pi_ / 2);
    }

    // Computes the signed angle in radians [-pi, pi] betwen the vectors relatively to
    // the base vector.
    RealType angle(const Point2D &base, const Point2D &v)
    {
        // Cosine between the vectors.
        RealType cosa = base * v / base.euclidean_norm() / v.euclidean_norm();

        // Angle without the sign.
        RealType a = std::acos(cosa);

        // Normal vector for the base.
        Point2D norm_base = normal(base);

        // The sign is defined by the halfspace relatively to base where v is located.
        int sign = norm_base * v < 0 ? -1 : 1;

        // Angle with sign.
        return sign * a;
    }

    // Computes the intersection of two lines, defined by the reference points and
    // the given element of the alpha-table.
    Point2D find_intersection(const Reference &reference_points, const ATableElement &e)
    {
        // Reference points.
        Point2D p1 = reference_points.first;
        Point2D p2 = reference_points.second;

        // Reference vector.
        Point2D ab = p2 - p1;

        // If the intersection is located on the reference line, compute the intersection
        // directly.
        if (e.first == 0)
        {
            return p1 + e.second * ab;
        }

        // Find the first direction with the base norm.
        Point2D v1 = rotate(ab, e.first);

        // Calculate the intersection point.
        RealType sinb = std::sin(e.second);
        RealType sinba = std::sin(e.second - e.first);

        // The intersection is not on the reference line and not in the infinity.
        BOOST_ASSERT(sinba != 0);

        // The coordinate of the intersection point relatively to v1.
        RealType t = sinb / sinba;

        return p1 + t * v1;
    }

    // Fills in the alpha-table using the given model features
    // and two reference points.
    void encode(const Features &model_features)
    {
        const RealType epsilon = RealType(0.001);

        // Define the number of discrete tangent angles.
        unsigned int tangent_angle_number = static_cast<unsigned int>(2 * pi_ / tangent_accuracy_);

        // Allocate memory for the alpha-table.
        atable_.resize(tangent_angle_number);

        // The reference vector.
        Point2D ab = model_reference_.second - model_reference_.first;

        // Fill in the alpha-table.
        for (typename Features::const_iterator it = model_features.begin();
             it != model_features.end(); ++it)
        {
            // Current model point.
            Point2D c = it->first;
            // Boundary tangent at the current model point.
            Point2D tangent = it->second;

            // Vectors from the reference points to the current model point.
            Point2D v1 = c - model_reference_.first;
            Point2D v2 = c - model_reference_.second;

            // Compute the reference angles.
            RealType alpha = angle(ab, v1);
            RealType beta = angle(ab, v2);
            // Compute the tangential angle.
            RealType gamma = angle(v1, tangent);

            // Correction for the points located on the reference line.
            // This case is encoded as: alpha = 0; beta = coordinate of the current model point
            // on the reference line relatively to the reference vector.
            if (std::abs(std::sin(beta)) < epsilon)
            {
                alpha = 0;
                RealType abnorm =  ab.euclidean_norm();
                beta = ab * v1 / (abnorm * abnorm);
            }

            // Define the row for the computed angles in the alpha-table and insert
            // the angles into the table.
            atable_.at(atable_index(gamma)).push_back(ATableElement(alpha, beta));
        }
    }

    void normalize_scaling_range(Point2D &scaling_range)
    {
        if (scaling_range[0] < 0)
            scaling_range[0] = 0;

        if (scaling_range[1] < 0)
            scaling_range[1] = 0;

        if (scaling_range[0] > scaling_range[1])
            std::swap(scaling_range[0], scaling_range[1]);
    }

    inline void subdivide_with_votes(Space4D &s, const Features &object_features,
                                     const Point2D &scaling_range)
    {
        // Create the space subdivision.
        s.subdivide();

        // Calculate the votes for the obtained subspaces.
        for (typename Space4D::Spaces::iterator it = s.get_subspaces().begin();
             it != s.get_subspaces().end(); ++it)
        {
            feature_to_vote(*it, object_features, scaling_range);
        }
    }

    // Recursively fills in the space tree calculating votes for each subspace.
    void process_space(Space4D &s, const Features &object_features, const Point2D &scaling_range,
                       RealType probability)
    {
        if (s.get_resolution_level() >= s.get_max_resolution_level())
            return;

        // Subdivide the space and compute votes for its subspaces.
        subdivide_with_votes(s, object_features, scaling_range);

        // Sorting subspaces in descending order!
        std::sort(s.get_subspaces().rbegin(), s.get_subspaces().rend());

        // The minimal number of spaces from the beginning of the space collection such
        // that the probability of maximal element is not less than the given value.
        std::size_t n = SubPolicy::probabilistic(s.get_subspaces(), probability);

        for (std::size_t i = 0; i < n; ++i)
        {
            // Continue the subdivision procedure recursively.
            process_space(s.get_subspaces().at(i), object_features, scaling_range, probability);
        }
    }

    // Intersects the given space with the lines produced by the object features and increase the
    // number of the space votes.
    void feature_to_vote(Space4D &s, const Features &object_features, const Point2D &scaling_range)
    {
        for (typename Features::const_iterator it = object_features.begin();
             it != object_features.end(); ++it)
        {
            feature_to_vote2(s, *it, scaling_range);
        }
    }

    // Intersects the given space with the line defined by the feature (position and
    // tangent) and increase the number of the space votes.
    inline void feature_to_vote(Space4D &s, const Feature &f, const Point2D &scaling_range)
    {
        // Reconstruct all the 4D lines from the alpha-table relatively to the current feature.
        for (std::size_t index = 0; index < atable_.size(); ++index)
        {
            // Probable angle between the directional vector and the tangent.
            RealType gamma = atable_gamma(index);

            // Find all corresponding directional vectors v2 defined by the
            // (alpha, beta) angles of the current table row.
            for (typename ATableRow::const_iterator abit = atable_.at(index).begin();
                 abit !=  atable_.at(index).end(); ++abit)
            {
                typename Space4D::Line4D line4 = line4_from_feature_and_atable_element(f,
                        gamma, *abit);

                // Create two segments on this line that correspond to the given scaling range.
                Point2D v1(line4.direction[0], line4.direction[1]);
                Point2D v2(line4.direction[2], line4.direction[3]);
                RealType vnorm = (v2 - v1).euclidean_norm();
                Point2D segment_coords = scaling_range * model_base_ / vnorm;

                typename Space4D::Segment4D segment1(line4,  segment_coords);
                typename Space4D::Segment4D segment2(line4, -segment_coords);

                // Intersect and compute the votes.
                s.vote_maxnorm(s.intersect(segment1));
                s.vote_maxnorm(s.intersect(segment2));
            }
        }
    }

    inline typename Space4D::Line4D line4_from_feature_and_atable_element(const Feature &f,
            RealType gamma, const ATableElement &e)
    {
        Point2D c = f.first;
        Point2D tan = f.second;

        // Normalized directional vector.
        Point2D v1 = rotate(tan, -gamma);
        v1 = v1 / v1.euclidean_norm();

        RealType alpha = e.first;
        RealType beta = e.second;

        Point2D v2 = rotate(v1, beta - alpha);

        // Consider the directional vectors ratio.
        if (alpha != 0)
            // The conventional case.
            v2 *= std::sin(alpha) / std::sin(beta);
        else
            // The directional vectors are located on the reference line.
            v2 *= 1 - 1 / beta;

        // Compose a 4D line.
        // The point on this line.
        typename Space4D::Point4D p4(c.x(), c.y(), c.x(), c.y());
        // The directional vector of the line.
        typename Space4D::Point4D v4(v1.x(), v1.y(), v2.x(), v2.y());
        // The line in 4D.
        typename Space4D::Line4D line4(p4, v4);

        return line4;
    }

    // Intersects the given space with the hyperplane defined by the feature
    // (position and tangent) and increase the number of the space votes.
    inline void feature_to_vote2(Space4D &s, const Feature &f, const Point2D &scaling_range)
    {
        // Reconstruct all the 4D hyperplanes from the alpha-table relatively to the current feature.
        for (std::size_t index = 0; index < atable_.size(); ++index)
        {
            // Probable angle between the directional vector and the tangent.
            RealType gamma = atable_gamma(index);

            // Find all corresponding directional vectors v2 defined by the (alpha, beta)
            // angles of the current table row.
            for (typename ATableRow::const_iterator abit = atable_.at(index).begin();
                 abit !=  atable_.at(index).end(); ++abit)
            {
                // Reconstruct the 4D line.
                typename Space4D::Line4D line4 = line4_from_feature_and_atable_element(f,
                        gamma, *abit);

                // Use the hyperplane paradigm.
                typename Space4D::Point4D norm(line4.direction[3], line4.direction[2],
                                            -line4.direction[1], -line4.direction[0]);
                typename Space4D::Point4D point = line4.point;
                typename Space4D::Hyperplane plane(point, norm);

                // Check the plane constrains here and vote.
                if (is_in_scaling_constrains(s, plane, scaling_range) &&
                    is_in_tangent_constrains(s, plane, RealType(0.3)))
                {
                     s.vote_descrete(plane);
                }
            }
        }
    }

    int location_by_line(const Point2D &bot, const Point2D &top,
                         const Point2D &b, const Point2D &normal) const
    {
        Point2D d = top - bot;

        RealType proj_min = std::numeric_limits<RealType>::max();
        RealType proj_max = -proj_min;

        for (std::size_t i = 0; i < 2; ++i)
            for (std::size_t j = 0; j < 2; ++j)
            {
                Point2D p = bot + Point2D(d[0] * i, d[1] * j);

                RealType proj = (p - b) * normal;

                if (proj_min > proj)
                    proj_min = proj;

                if (proj_max < proj)
                    proj_max = proj;
            }

        if ((proj_min == 0 && proj_max == 0) || proj_min * proj_max < 0)
            return 0;

        return proj_max > 0 ? 1 : -1;
    }

    bool is_in_scaling_constrains(const Space4D &s,
                                  const typename Space4D::Hyperplane &plane,
                                  const Point2D &scaling_range) const
    {
        // Compute directional vectors.
        Point2D v1(-plane.normal()[3], -plane.normal()[2]);
        Point2D v2(plane.normal()[1], plane.normal()[0]);

        RealType vnorm = (v2 - v1).euclidean_norm();
        Point2D scaling = scaling_range * model_base_ / vnorm;

        typename Space4D::Box4D box = s.get_bounding_box();

        // Box projections.
        Point2D bot_v1(box.first[0], box.first[1]);
        Point2D top_v1(box.second[0], box.second[1]);
        Point2D bot_v2(box.first[2], box.first[3]);
        Point2D top_v2(box.second[2], box.second[3]);

        Point2D b(plane.point()[0], plane.point()[1]);

        // Points on the lines.
        Point2D b_v1q1 = b + v1 * scaling[0];
        Point2D b_v1q2 = b + v1 * scaling[1];
        Point2D b_v2q1 = b + v2 * scaling[0];
        Point2D b_v2q2 = b + v2 * scaling[1];

        Point2D b_v1q1_inv = b - v1 * scaling[0];
        Point2D b_v1q2_inv = b - v1 * scaling[1];
        Point2D b_v2q1_inv = b - v2 * scaling[0];
        Point2D b_v2q2_inv = b - v2 * scaling[1];

        return (location_by_line(bot_v1, top_v1, b_v1q1, v1) >= 0 &&
                location_by_line(bot_v1, top_v1, b_v1q2, v1) <= 0 &&
                location_by_line(bot_v2, top_v2, b_v2q1, v2) >= 0 &&
                location_by_line(bot_v2, top_v2, b_v2q2, v2) <= 0) ||
               (location_by_line(bot_v1, top_v1, b_v1q1_inv, -v1) >= 0 &&
                location_by_line(bot_v1, top_v1, b_v1q2_inv, -v1) <= 0 &&
                location_by_line(bot_v2, top_v2, b_v2q1_inv, -v2) >= 0 &&
                location_by_line(bot_v2, top_v2, b_v2q2_inv, -v2) <= 0);
    }

    inline void sector_constraint(const Point2D &v, RealType sigma,
                                  Point2D &nw) const
    {
        Point2D w = rotate(v, sigma);

        nw[0] = -w[1];
        nw[1] = w[0];

        // Correct the sign of the normal.
        if (nw * (v - w) > 0)
            nw *= -1;
    }

    bool is_in_tangent_constrains(const Space4D &s,
                                  const typename Space4D::Hyperplane &plane,
                                  const RealType sigma) const
    {
        // Compute directional vectors v1 and v2.
        Point2D v1(-plane.normal()[3], -plane.normal()[2]);
        Point2D v2(plane.normal()[1], plane.normal()[0]);

        typename Space4D::Box4D box = s.get_bounding_box();

        // Box projections.
        Point2D bot_v1(box.first[0], box.first[1]);
        Point2D top_v1(box.second[0], box.second[1]);
        Point2D bot_v2(box.first[2], box.first[3]);
        Point2D top_v2(box.second[2], box.second[3]);

        Point2D b(plane.point()[0], plane.point()[1]);

        // Calculate vectors constraining the sigma-sector.
        Point2D nw1, nw2, nu1, nu2;
        sector_constraint(v1, sigma, nw1);
        sector_constraint(v1, -sigma, nu1);
        sector_constraint(v2, sigma, nw2);
        sector_constraint(v2, -sigma, nu2);

        return (location_by_line(bot_v1, top_v1, b, nw1) *
                location_by_line(bot_v1, top_v1, b, nu1) >= 0) &&
               (location_by_line(bot_v2, top_v2, b, nw2) *
                location_by_line(bot_v2, top_v2, b, nu2) >= 0);
    }

    // Recursively traces the space tree and inserts into the container the elements from
    // the given resolution level.
    void get_resolution_level(const Space4D &s, typename Space4D::Spaces &container,
                              std::size_t resolution_level)
    {
        if (s.get_resolution_level() == resolution_level)
            container.push_back(s);
        else
        {
            const typename Space4D::Spaces subs = s.get_subspaces();

            for (typename Space4D::Spaces::const_iterator it = subs.begin();
                 it != subs.end(); ++it)
            {
                get_resolution_level(*it, container, resolution_level);
            }
        }

    }

};

} // namespace recognition
} // namespace bo

#endif // DPG_HOUGH_TRANSFORM_HPP_55B0255B_E6C8_4302_9114_D1B684CD3419_