#include <boost/test/unit_test.hpp>

#include <madym/tests/mdm_test_utils.h>

#include <fstream>
#include <madym/t1_methods/mdm_T1FitterVFA.h>
#include <madym/t1_methods/mdm_T1FitterIR.h>
#include <mdm_version.h>

namespace fs = boost::filesystem;

void run_T1_lite_VFA_test(double B1)
{
  //Generate some signals from sample FA, TR, T1 and M0 values
  double T1 = 1000;
  double M0 = 2000;
  double TR = 3.5;
  const auto PI = acos(-1.0);
  std::vector<double>	FAs = { 2 , 10, 18 };

  std::string test_dir = mdm_test_utils::temp_dir();
  std::string inputDataFile = test_dir + "/T1_input.dat";
  std::ofstream ifs(inputDataFile, std::ios::out);

  BOOST_REQUIRE_MESSAGE(ifs.is_open(), "Failed to write out test values for T1");

  if (B1)
  {
    //Write out FAs
    for (const auto FA : FAs)
      ifs << FA/B1 << " ";
  }
  else
  {
    //Write out FAs
    for (const auto FA : FAs)
      ifs << FA << " ";
  }

  //Compute signal for each FA and write out value
  for (int i_fa = 0; i_fa < 3; i_fa++)
    ifs << mdm_T1FitterVFA::T1toSignal(T1, M0, PI*FAs[i_fa] / 180, TR) << " ";

  if (B1)
    ifs << B1;

  ifs.close();

  std::string method;
  if (B1)
  {
    BOOST_TEST_MESSAGE("Testing VFA with B1 correction");
    method = "VFA_B1";
  }

  else
  {
    BOOST_TEST_MESSAGE("Testing VFA");
    method = "VFA";
  }

  //Call calculate_T1 to fit T1 and M0
  std::string T1_output_dir = test_dir + "/madym_T1_lite/";
  std::string outputName = "madym_analysis.dat";
  std::stringstream cmd;
  cmd << mdm_test_utils::tools_exe_dir() << "madym_T1_lite"
    << " -T " << method
    << " --data " << inputDataFile
    << " --n_T1 " << 3
    << " --TR " << TR
    << " -o " << T1_output_dir
    << " -O " << outputName;

  BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

  int error;
  try
  {
    error = std::system(cmd.str().c_str());
  }
  catch (...)
  {
    BOOST_CHECK_MESSAGE(false, "Running madym_T1_lite failed");
    return;
  }

  BOOST_CHECK_MESSAGE(!error, "Error returned from madym_T1_lite tool");

  //Load in the fitted parameters from the output file
  std::string outputDataFile = T1_output_dir + method + "_" + outputName;
  std::ifstream ofs(outputDataFile, std::ios::in);
  BOOST_REQUIRE_MESSAGE(ofs.is_open(), "Failed to read in fitted values for T1");

  double T1_fit, M0_fit;
  int fit_errors;
  ofs >> T1_fit;
  ofs >> M0_fit;
  ofs >> fit_errors;
  ofs.close();

  //Check the model parameters have fitted correctly
  double tol = 0.1;
  BOOST_TEST_MESSAGE("Testing fitted T1");
  BOOST_CHECK_CLOSE(T1_fit, T1, tol);
  BOOST_TEST_MESSAGE("Testing fitted M0");
  BOOST_CHECK_CLOSE(M0_fit, M0, tol);
  BOOST_TEST_MESSAGE("Checking zero error-codes");
  BOOST_CHECK(!fit_errors);

  //Tidy up
  fs::remove(inputDataFile);
  fs::remove_all(T1_output_dir);
}

void run_T1_lite_IR_test()
{
  //Generate some signals from sample FA, TR, T1 and M0 values
 //Generate some signals from sample FA, TR, T1 and S0 values
  std::vector<double> TIs = { 50, 300, 800, 1000, 2000, 4000 };
  int nTIs = (int)TIs.size();
  double T1 = 1200;
  double M0 = 1200;
  double TR = 1e5;

  std::string test_dir = mdm_test_utils::temp_dir();
  std::string inputDataFile = test_dir + "/T1_input.dat";
  std::ofstream ifs(inputDataFile, std::ios::out);

  BOOST_REQUIRE_MESSAGE(ifs.is_open(), "Failed to write out test values for T1");

  //Write out TIs
  for (const auto ti : TIs)
    ifs << ti << " ";

  //Compute signal for each IR and write out value
  for (int i = 0; i < nTIs; i++)
    ifs << mdm_T1FitterIR::T1toSignal(T1, M0, TIs[i], TR) << " ";

  ifs.close();

  std::string method;
  BOOST_TEST_MESSAGE("Testing IR");
  method = "IR";

  //Call calculate_T1 to fit T1 and M0
  std::string T1_output_dir = test_dir + "/madym_T1_lite/";
  std::string outputName = "madym_analysis.dat";
  std::stringstream cmd;
  cmd << mdm_test_utils::tools_exe_dir() << "madym_T1_lite"
    << " -T " << method
    << " --data " << inputDataFile
    << " --n_T1 " << nTIs
    << " --TR " << TR
    << " -o " << T1_output_dir
    << " -O " << outputName;

  BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

  int error;
  try
  {
    error = std::system(cmd.str().c_str());
  }
  catch (...)
  {
    BOOST_CHECK_MESSAGE(false, "Running madym_T1_lite failed");
    return;
  }

  BOOST_CHECK_MESSAGE(!error, "Error returned from madym_T1_lite tool");

  //Load in the fitted parameters from the output file
  std::string outputDataFile = T1_output_dir + method + "_" + outputName;
  std::ifstream ofs(outputDataFile, std::ios::in);
  BOOST_REQUIRE_MESSAGE(ofs.is_open(), "Failed to read in fitted values for T1");

  double T1_fit, M0_fit;
  int fit_errors;
  ofs >> T1_fit;
  ofs >> M0_fit;
  ofs >> fit_errors;
  ofs.close();

  //Check the model parameters have fitted correctly
  double tol = 0.1;
  BOOST_TEST_MESSAGE("Testing fitted T1");
  BOOST_CHECK_CLOSE(T1_fit, T1, tol);
  BOOST_TEST_MESSAGE("Testing fitted M0");
  BOOST_CHECK_CLOSE(M0_fit, M0, tol);
  BOOST_TEST_MESSAGE("Checking zero error-codes");
  BOOST_CHECK(!fit_errors);

  //Tidy up
  fs::remove(inputDataFile);
  fs::remove_all(T1_output_dir);
}

BOOST_AUTO_TEST_SUITE(test_mdm_tools)

BOOST_AUTO_TEST_CASE(test_madym_T1_lite) {
	BOOST_TEST_MESSAGE("======= Testing tool: madym T1 lite =======");

  //Run VFA fit with no B1 correction
  run_T1_lite_VFA_test(0);

  //Run VFA fit with B1 correction
  run_T1_lite_VFA_test(0.9);

  //Run IR fit
  run_T1_lite_IR_test();

}

BOOST_AUTO_TEST_SUITE_END() //


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef SPHERICALDIFFUSE_HPP
#define SPHERICALDIFFUSE_HPP

#include <cmath>
#include <iosfwd>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

// Has to be included here
#include "InterfacesImpl.hpp"
// Boost.Math includes
#include <boost/math/special_functions/legendre.hpp>

#include "DerivativeUtils.hpp"
#include "bi_operators/IntegratorForward.hpp"
#include "dielectric_profile/ProfileForward.hpp"
#include "GreensFunction.hpp"
#include "utils/MathUtils.hpp"

/*! \file SphericalDiffuse.hpp
 *  \class SphericalDiffuse
 *  \brief Green's function for a diffuse interface with spherical symmetry
 *  \author Hui Cao, Ville Weijo, Luca Frediani and Roberto Di Remigio
 *  \date 2010-2015
 *  \tparam IntegratorPolicy policy for the calculation of the matrix represenation of S and D
 *  \tparam ProfilePolicy functional form of the diffuse layer
 *
 *  This class is general, in the sense that no specific dielectric profile has been set in its definition.
 *  In principle any profile that can be described by:
 *  1. a left-side dielectric constant;
 *  2. a right-side dielectric constant;
 *  3. an interface layer width;
 *  4. an interface layer center
 *  can be used to define a new diffuse interface with spherical symmetry.
 *  The origin of the dielectric sphere can be changed by means of the constructor.
 *  The solution of the differential equation defining the Green's function is **always**
 *  performed assuming that the dielectric sphere is centered in the origin of the coordinate
 *  system. Whenever the public methods are invoked to "sample" the Green's function
 *  at a pair of points, a translation of the sampling points is performed first.
 */

template <typename IntegratorPolicy = CollocationIntegrator,
          typename ProfilePolicy = OneLayerTanh>
class SphericalDiffuse __final : public GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy,
                                               SphericalDiffuse<IntegratorPolicy, ProfilePolicy> >
{
public:
    /*! Constructor for a one-layer interface
     * \param[in] e1 left-side dielectric constant
     * \param[in] e2 right-side dielectric constant
     * \param[in] w width of the interface layer
     * \param[in] c center of the diffuse layer
     * \param[in] o center of the sphere
     */
    SphericalDiffuse(double e1, double e2, double w, double c, const Eigen::Vector3d & o, int l)
        : GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, SphericalDiffuse<IntegratorPolicy, ProfilePolicy> >(),
          origin_(o), maxLGreen_(l), maxLC_(2*l)
    {
        initProfilePolicy(e1, e2, w, c);
        initSphericalDiffuse();
    }
    /*! Constructor for a one-layer interface
     * \param[in] e1 left-side dielectric constant
     * \param[in] e2 right-side dielectric constant
     * \param[in] w width of the interface layer
     * \param[in] c center of the diffuse layer
     * \param[in] o center of the sphere
     */
    SphericalDiffuse(double e1, double e2, double w, double c, const Eigen::Vector3d & o, int l, double f)
        : GreensFunction<Numerical, IntegratorPolicy, ProfilePolicy, SphericalDiffuse<IntegratorPolicy, ProfilePolicy> >(f),
          origin_(o), maxLGreen_(l), maxLC_(2*l)
    {
        initProfilePolicy(e1, e2, w, c);
        initSphericalDiffuse();
    }
    virtual ~SphericalDiffuse() {}

    /*! Calculates the matrix representation of the S operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd singleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.singleLayer(*this, e);
    }
    /*! Calculates the matrix representation of the D operator
     *  \param[in] e list of finite elements
     */
    virtual Eigen::MatrixXd doubleLayer(const std::vector<Element> & e) const __override
    {
        return this->integrator_.doubleLayer(*this, e);
    }

    friend std::ostream & operator<<(std::ostream & os, SphericalDiffuse & gf) {
        return gf.printObject(os);
    }
    /*! \brief Returns Coulomb singularity separation coefficient
     *  \param[in] source location of the source charge
     *  \param[in] probe location of the probe charge
     */
    double coefficientCoulomb(const Eigen::Vector3d & source, const Eigen::Vector3d & probe) const {
        // Obtain coefficient for the separation of the Coulomb singularity
        return this->coefficient_impl(source, probe);
    }
    /*! \brief Returns singular part of the Green's function
     *  \param[in] source location of the source charge
     *  \param[in] probe location of the probe charge
     */
    double Coulomb(const Eigen::Vector3d & source, const Eigen::Vector3d & probe) const {
        double r12 = (source - probe).norm();

        // Obtain coefficient for the separation of the Coulomb singularity
        return (1.0 / (this->coefficient_impl(source, probe) * r12));
    }
    /*! \brief Returns non-singular part of the Green's function (image potential)
     *  \param[in] source location of the source charge
     *  \param[in] probe location of the probe charge
     */
    double imagePotential(const Eigen::Vector3d & source, const Eigen::Vector3d & probe) const {
        // Obtain coefficient for the separation of the Coulomb singularity
        double Cr12 = this->coefficient_impl(source, probe);

        double gr12 = 0.0;
        for (int L = 1; L <= maxLGreen_; ++L) {
            gr12 += this->imagePotentialComponent_impl(L, source, probe, Cr12);
        }

        return gr12;
    }
    /*! Returns value of the directional derivative of the
     *  Coulomb singularity separation coefficient for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point, thus assuming that the direction is relative to that point.
     *
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    double coefficientCoulombDerivative(const Eigen::Vector3d & direction, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const {
        return threePointStencil(pcm::bind(&SphericalDiffuse<IntegratorPolicy, ProfilePolicy>::coefficientCoulomb, this, pcm::_1, pcm::_2),
                p2, p1, direction, this->delta_);
    }
    /*! Returns value of the directional derivative of the
     *  singular part of the Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point, thus assuming that the direction is relative to that point.
     *
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    double CoulombDerivative(const Eigen::Vector3d & direction, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const {
        return threePointStencil(pcm::bind(&SphericalDiffuse<IntegratorPolicy, ProfilePolicy>::Coulomb, this, pcm::_1, pcm::_2),
                p2, p1, direction, this->delta_);
    }
    /*! Returns value of the directional derivative of the
     *  non-singular part (image potential) of the Greens's function for the pair of points p1, p2:
     *  \f$ \nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  Notice that this method returns the directional derivative with respect
     *  to the probe point, thus assuming that the direction is relative to that point.
     *
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    double imagePotentialDerivative(const Eigen::Vector3d & direction, const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const {
        return threePointStencil(pcm::bind(&SphericalDiffuse<IntegratorPolicy, ProfilePolicy>::imagePotential, this, pcm::_1, pcm::_2),
                p2, p1, direction, this->delta_);
    }
    /*! Handle to the dielectric profile evaluation */
    pcm::tuple<double, double> epsilon(const Eigen::Vector3d & point) const {
        return this->profile_((point + this->origin_).norm());
    }
    void toFile(const std::string & prefix = "") {
	std::string tmp;
	prefix.empty() ? tmp = prefix : tmp = prefix + "-";
        writeToFile(zetaC_, tmp + "zetaC.dat");
        writeToFile(omegaC_, tmp + "omegaC.dat");
	for (int L = 1; L <= maxLGreen_; ++L) {
	    writeToFile(zeta_[L], tmp + "zeta_" + pcm::to_string(L) + ".dat");
	    writeToFile(omega_[L], tmp + "omega_" + pcm::to_string(L) + ".dat");
	}
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW /* See http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html */
private:
    /*! Evaluates the Green's function given a pair of points
     *  \param[in] sp the source point
     *  \param[in] pp the probe point
     *
     *  \note This takes care of the origin shift
     */
    virtual Numerical operator()(Numerical * sp, Numerical * pp) const __override
    {
        // Transfer raw arrays to Eigen vectors using the Map type
        Eigen::Map<Eigen::Matrix<double, 3, 1> > source(sp), probe(pp);

        // Obtain coefficient for the separation of the Coulomb singularity
        double Cr12 = this->coefficient_impl(source, probe);

        double gr12 = 0.0;
        for (int L = 1; L <= maxLGreen_; ++L) {
            gr12 += this->imagePotentialComponent_impl(L, source, probe, Cr12);
        }
        double r12 = (source - probe).norm();

        return (1.0 / (Cr12 * r12) + gr12);
    }
    /*! Returns value of the kernel of the \f$\mathcal{D}\f$ integral operator for the pair of points p1, p2:
     *  \f$ [\boldsymbol{\varepsilon}\nabla_{\mathbf{p_2}}G(\mathbf{p}_1, \mathbf{p}_2)]\cdot \mathbf{n}_{\mathbf{p}_2}\f$
     *  To obtain the kernel of the \f$\mathcal{D}^\dagger\f$ operator call this methods with \f$\mathbf{p}_1\f$
     *  and \f$\mathbf{p}_2\f$ exchanged and with \f$\mathbf{n}_{\mathbf{p}_2} = \mathbf{n}_{\mathbf{p}_1}\f$
     *  \param[in] direction the direction
     *  \param[in]        p1 first point
     *  \param[in]        p2 second point
     */
    virtual double kernelD_impl(const Eigen::Vector3d & direction,
                              const Eigen::Vector3d & p1, const Eigen::Vector3d & p2) const __override
    {
        double eps_r2 = 0.0;
        // Shift p2 by origin_
        pcm::tie(eps_r2, pcm::ignore) = this->epsilon(p2);

        return (eps_r2 * this->derivativeProbe(direction, p1, p2));
    }
    virtual KernelS exportKernelS_impl() const __override {
      return pcm::bind(&SphericalDiffuse<IntegratorPolicy, ProfilePolicy>::kernelS, *this, pcm::_1, pcm::_2);
    }
    virtual KernelD exportKernelD_impl() const __override {
      return pcm::bind(&SphericalDiffuse<IntegratorPolicy, ProfilePolicy>::kernelD, *this, pcm::_1, pcm::_2, pcm::_3);
    }
    virtual std::ostream & printObject(std::ostream & os) __override
    {
        Eigen::IOFormat CleanFmt(Eigen::StreamPrecision, 0, ", ", "\n", "(", ")");
        os << "Green's function type: spherical diffuse" << std::endl;
        os << this->profile_ << std::endl;
        os << "Sphere center        = " << this->origin_.transpose().format(CleanFmt) << std::endl;
        os << "Angular momentum (Green's function)    = " << this->maxLGreen_ << std::endl;
        os << "Angular momentum (Coulomb coefficient) = " << this->maxLC_;
        return os;
    }
    /*! Initializes a one-layer profile
     *  \param[in] e1 left-side dielectric constant
     *  \param[in] e2 right-side dielectric constant
     *  \param[in] w width of the interface layer
     *  \param[in] c center of the diffuse layer
     */
    void initProfilePolicy(double e1, double e2, double w, double c) {
        this->profile_ = ProfilePolicy(e1, e2, w, c);
    }
    /*! This calculates all the components needed to evaluate the Green's function */
    void initSphericalDiffuse() {
        using namespace interfaces;

        LOG("SphericalDiffuse::initSphericalDiffuse");
        // Parameters for the numerical solution of the radial differential equation
        double eps_abs_     = 1.0e-10; /*! Absolute tolerance level */
        double eps_rel_     = 1.0e-06; /*! Relative tolerance level */
        double factor_x_    = 0.0;     /*! Weight of the state      */
        double factor_dxdt_ = 0.0;     /*! Weight of the state derivative */
        double r_0_         = 0.5;     /*! Lower bound of the integration interval */
        double r_infinity_  = this->profile_.center() + 200.0; /*! Upper bound of the integration interval */
        double observer_step_ = 1.0e-03; /*! Time step between observer calls */
        IntegratorParameters params_(eps_abs_, eps_rel_, factor_x_, factor_dxdt_, r_0_, r_infinity_, observer_step_);
        ProfileEvaluator eval_ = pcm::bind(&ProfilePolicy::operator(), this->profile_, pcm::_1);

        LOG("Computing coefficient for the separation of the Coulomb singularity");
        LOG("Computing first radial solution L = " + pcm::to_string(maxLC_));
        TIMER_ON("computeZeta for coefficient");
        zetaC_ = RadialFunction<StateType, LnTransformedRadial, Zeta>(maxLC_, r_0_, r_infinity_, eval_, params_);
        TIMER_OFF("computeZeta for coefficient");
        LOG("DONE: Computing first radial solution L = " + pcm::to_string(maxLC_));

        LOG("Computing second radial solution L = " + pcm::to_string(maxLC_));
        TIMER_ON("computeOmega for coefficient");
        omegaC_ = RadialFunction<StateType, LnTransformedRadial, Omega>(maxLC_, r_0_, r_infinity_, eval_, params_);
        TIMER_OFF("computeOmega for coefficient");
        LOG("Computing second radial solution L = " + pcm::to_string(maxLC_));
        LOG("DONE: Computing coefficient for the separation of the Coulomb singularity");

        LOG("Computing radial solutions for Green's function");
        TIMER_ON("SphericalDiffuse: Looping over angular momentum");
        zeta_.reserve(maxLGreen_+1);
        omega_.reserve(maxLGreen_+1);
        for (int L = 0; L <= maxLGreen_; ++L) {
            // First radial solution
            LOG("Computing first radial solution L = " + pcm::to_string(L));
            TIMER_ON("computeZeta L = " + pcm::to_string(L));
            // Create an empty RadialSolution
            RadialFunction<StateType, LnTransformedRadial, Zeta> tmp_zeta_(L, r_0_, r_infinity_, eval_, params_);
            zeta_.push_back(tmp_zeta_);
            TIMER_OFF("computeZeta L = " + pcm::to_string(L));
            LOG("DONE: Computing first radial solution L = " + pcm::to_string(L));

            // Second radial solution
            LOG("Computing second radial solution L = " + pcm::to_string(L));
            TIMER_ON("computeOmega L = " + pcm::to_string(L));
            // Create an empty RadialSolution
            RadialFunction<StateType, LnTransformedRadial, Omega> tmp_omega_(L, r_0_, r_infinity_, eval_, params_);
            omega_.push_back(tmp_omega_);
            TIMER_OFF("computeOmega L = " + pcm::to_string(L));
            LOG("DONE: Computing second radial solution L = " + pcm::to_string(L));
        }
        TIMER_OFF("SphericalDiffuse: Looping over angular momentum");
        LOG("DONE: Computing radial solutions for Green's function");
    }

    /*! Center of the dielectric sphere */
    Eigen::Vector3d origin_;

    /**@{ Parameters and functions for the calculation of the Green's function, including Coulomb singularity */
    /*! Maximum angular momentum in the __final summation over Legendre polynomials to obtain G */
    int maxLGreen_;
    /*! \brief First independent radial solution, used to build Green's function.
     *  \note The vector has dimension maxLGreen_ and has r^l behavior
     */
    std::vector<RadialFunction<interfaces::StateType, interfaces::LnTransformedRadial, Zeta> > zeta_;
    /*! \brief Second independent radial solution, used to build Green's function.
     *  \note The vector has dimension maxLGreen_  and has r^(-l-1) behavior
     */
    std::vector<RadialFunction<interfaces::StateType, interfaces::LnTransformedRadial, Omega> > omega_;
    /*! \brief Returns L-th component of the radial part of the Green's function
     *  \param[in] L  angular momentum
     *  \param[in] sp source point
     *  \param[in] pp probe point
     *  \param[in] Cr12 Coulomb singularity separation coefficient
     *  \note This function shifts the given source and probe points by the location of the
     *  dielectric sphere.
     */
    double imagePotentialComponent_impl(int L, const Eigen::Vector3d & sp, const Eigen::Vector3d & pp, double Cr12) const {
        Eigen::Vector3d sp_shift = sp + this->origin_;
        Eigen::Vector3d pp_shift = pp + this->origin_;
        double r1 = sp_shift.norm();
        double r2 = pp_shift.norm();
        double cos_gamma = sp_shift.dot(pp_shift) / (r1 * r2);
        // Evaluate Legendre polynomial of order L
        // First of all clean-up cos_gamma, Legendre polynomials
        // are only defined for -1 <= x <= 1
        if (numericalZero(cos_gamma - 1)) cos_gamma = 1.0;
        if (numericalZero(cos_gamma + 1)) cos_gamma = -1.0;
        double pl_x = boost::math::legendre_p(L, cos_gamma);

        /* Sample zeta_[L] */
        double zeta1 = 0.0, zeta2 = 0.0, d_zeta2 = 0.0;
        /* Value of zeta_[L] at point with index 1 */
        pcm::tie(zeta1, pcm::ignore) = zeta_[L](r1);
        /* Value of zeta_[L] and its first derivative at point with index 2 */
        pcm::tie(zeta2, d_zeta2) = zeta_[L](r2);

        /* Sample omega_[L] */
        double omega1 = 0.0, omega2 = 0.0, d_omega2 = 0.0;
        /* Value of omega_[L] at point with index 1 */
        pcm::tie(omega1, pcm::ignore) = omega_[L](r1);
        /* Value of omega_[L] and its first derivative at point with index 2 */
        pcm::tie(omega2, d_omega2) = omega_[L](r2);

        double eps_r2 = 0.0;
        pcm::tie(eps_r2, pcm::ignore) = this->profile_(pp_shift.norm());

        /* Evaluation of the Wronskian and the denominator */
        double denominator = (d_zeta2 - d_omega2) * std::pow(r2, 2) * eps_r2;

        double gr12 = 0.0;
        if (r1 < r2) {
            gr12 = std::exp(zeta1 - zeta2) * (2*L +1) / denominator;
	    double f_L = r1 / r2;
	    for (int i = 1; i < L; ++i) { f_L *= r1 / r2; }
            gr12 = (gr12 - f_L / (r2 * Cr12) ) * pl_x ;
        } else {
            gr12 = std::exp(omega1 - omega2) * (2*L +1) / denominator;
	    double f_L = r2 / r1;
	    for (int i = 1; i < L; ++i) { f_L *= r2 / r1; }
            gr12 = (gr12 - f_L / (r1 * Cr12) ) * pl_x ;
        }

        return gr12;
    }
    /**@}*/

    /**@{ Parameters and functions for the calculation of the Coulomb singularity separation coefficient */
    /*! Maximum angular momentum to obtain C(r, r'), needed to separate the Coulomb singularity */
    int maxLC_; // = 2 * maxLGreen_;
    /*! \brief First independent radial solution, used to build coefficient.
     *  \note This is needed to separate the Coulomb singularity and has r^l behavior
     */
    RadialFunction<interfaces::StateType, interfaces::LnTransformedRadial, Zeta> zetaC_;
    /*! \brief Second independent radial solution, used to build coefficient.
     *  \note This is needed to separate the Coulomb singularity and has r^(-l-1) behavior
     */
    RadialFunction<interfaces::StateType, interfaces::LnTransformedRadial, Omega> omegaC_;
    /*! \brief Returns coefficient for the separation of the Coulomb singularity
     *  \param[in] sp first point
     *  \param[in] pp second point
     *  \note This function shifts the given source and probe points by the location of the
     *  dielectric sphere.
     */
    double coefficient_impl(const Eigen::Vector3d & sp, const Eigen::Vector3d & pp) const {
        double r1 = (sp + this->origin_).norm();
        double r2 = (pp + this->origin_).norm();

        /* Sample zetaC_ */
        double zeta1 = 0.0, zeta2 = 0.0, d_zeta2 = 0.0;
        /* Value of zetaC_ at point with index 1 */
        pcm::tie(zeta1, pcm::ignore) = zetaC_(r1);
        /* Value of zetaC_ and its first derivative at point with index 2 */
        pcm::tie(zeta2, d_zeta2) = zetaC_(r2);

        /* Sample omegaC_ */
        double omega1 = 0.0, omega2 = 0.0, d_omega2 = 0.0;
        /* Value of omegaC_ at point with index 1 */
        pcm::tie(omega1, pcm::ignore) = omegaC_(r1);
        /* Value of omegaC_ and its first derivative at point with index 2 */
        pcm::tie(omega2, d_omega2) = omegaC_(r2);

        double tmp = 0.0, coeff = 0.0;
        double eps_r2 = 0.0;
        pcm::tie(eps_r2, pcm::ignore) = this->profile_(r2);

        /* Evaluation of the Wronskian and the denominator */
        double denominator = (d_zeta2 - d_omega2) * std::pow(r2, 2) * eps_r2;

        if (r1 < r2) {
	    double f_L = r1 / r2;
	    for (int i = 1; i < maxLC_; ++i) { f_L *= r1 / r2; }
            tmp = std::exp(zeta1 - zeta2) * (2*maxLC_ +1) / denominator;
            coeff = f_L / (tmp * r2);
        } else {
	    double f_L = r2 / r1;
	    for (int i = 1; i < maxLC_; ++i) { f_L *= r2 / r1; }
            tmp = std::exp(omega1 - omega2) * (2*maxLC_ +1) / denominator;
            coeff = f_L / (tmp * r1);
        }

        return coeff;
    }
    /**@}*/
};

#endif // SPHERICALDIFFUSE_HPP


#include "CALPHADFreeEnergyFunctionsTernary.h"

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

namespace pt = boost::property_tree;

int main(int argc, char* argv[])
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::LINEAR;

    double temperature = 1570;
    std::cout << "Temperature: " << temperature << std::endl;
    double nominalc[2] = { 0.6, 0.035 };

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("calphadFeNbNi_Mathon_et_al.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsTernary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    const int nphi = 20;

    const double dphi = 1. / (double)nphi;

    // initial guesses
    double init_guess[5] = { 0.5, 0.05, 0.6, 0.01, 0.5 };
    double lceq[5]       = { init_guess[0], init_guess[1], // liquid
        init_guess[2], init_guess[3], // solid
        init_guess[4] };
    std::cout << "Tie line" << std::endl;
    bool found_ceq
        = cafe.computeTieLine(temperature, nominalc[0], nominalc[1], &lceq[0]);
    if (found_ceq)
    {
        std::cout << "Found solution: " << lceq[0] << ", " << lceq[1] << ", "
                  << lceq[2] << ", " << lceq[3] << ", " << lceq[4] << "..."
                  << std::endl;
    }

    // use tie line values as initial guesses
    for (int i = 0; i < 4; i++)
        init_guess[i] = lceq[i];

    for (int i = 0; i < nphi + 1; i++)
    {
        double phi = i * dphi;

        double conc[4] = { init_guess[0], init_guess[1], // liquid
            init_guess[2], init_guess[3] }; // solid

        cafe.computePhaseConcentrations(temperature, nominalc, &phi, conc);
        std::cout << "phi=" << phi << ", nominalc=" << nominalc[0] << ", "
                  << nominalc[1] << std::endl;
        std::cout << "KKS solution: cl = (" << conc[0] << "." << conc[1] << ")"
                  << " and cs = (" << conc[2] << "," << conc[3] << ")"
                  << std::endl;
    }
}


/// @file
///
/// Copyright Matus Chochlik.
/// Distributed under the Boost Software License, Version 1.0.
/// See accompanying file LICENSE_1_0.txt or copy at
///  http://www.boost.org/LICENSE_1_0.txt
///

#if EAGINE_USE_BOOST_SPIRIT
#if __has_include(<boost/spirit/home/x3.hpp>)
#include <eagine/assert.hpp>
#include <eagine/math/constants.hpp>
#include <boost/spirit/home/x3.hpp>
#include <complex>
#include <limits>
#include <memory>
#include <tuple>
#include <utility>
#else
#undef EAGINE_USE_BOOST_SPIRIT
#endif
#endif

#ifndef EAGINE_USE_BOOST_SPIRIT
#define EAGINE_USE_BOOST_SPIRIT 0
#endif

namespace eagine {
#if EAGINE_USE_BOOST_SPIRIT
namespace numexpr {
//------------------------------------------------------------------------------
namespace bs = boost::spirit::x3;
namespace bf = boost::fusion;
using bf::at_c;
using bs::_attr;
using bs::_val;
using parsed_number_t = std::complex<long double>;
//------------------------------------------------------------------------------
struct symbolic_constants : bs::symbols<parsed_number_t> {
    symbolic_constants() noexcept {
        add("pi", parsed_number_t(math::pi))("i", parsed_number_t(0, 1));
    }
};
//------------------------------------------------------------------------------
const symbolic_constants symbolic_constant{};

struct numeric_expression_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<numeric_expression_tag, parsed_number_t> numeric_expression =
  "numeric-expression";

struct function_call_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<function_call_tag, parsed_number_t> function_call =
  "function-call";

struct numeric_literal_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<numeric_literal_tag, parsed_number_t> numeric_literal =
  "numeric-literal";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto numeric_literal_def = bs::double_ | bs::int_;

//------------------------------------------------------------------------------
struct juxtaposition_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<juxtaposition_tag, parsed_number_t> juxtaposition =
  "juxtaposition";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto juxtaposition_def = (numeric_literal >> symbolic_constant)[(
  [](auto& c) { _val(c) = at_c<0>(_attr(c)) * at_c<1>(_attr(c)); })];
//------------------------------------------------------------------------------
struct conversion_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<conversion_tag, parsed_number_t> conversion = "conversion";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto conversion_def =
  (numeric_literal >> "deg")[([](auto& c) {
      _val(c) = ((parsed_number_t(math::pi) * _attr(c)) / parsed_number_t(180));
  })] |
  (numeric_literal >> "turn")[(
    [](auto& c) { _val(c) = (parsed_number_t(2 * math::pi) * _attr(c)); })];
//------------------------------------------------------------------------------
struct numeric_constant_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<numeric_constant_tag, parsed_number_t> numeric_constant =
  "numeric-constant";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto numeric_constant_def =
  (juxtaposition | conversion | numeric_literal | symbolic_constant);
//------------------------------------------------------------------------------
struct coersion_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<coersion_tag, parsed_number_t> coersion = "coersion";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto coersion_def =
  (numeric_constant)[([](auto& c) { _val(c) = _attr(c); })] |
  ("-" >> numeric_constant)[([](auto& c) { _val(c) = -_attr(c); })] |
  (function_call)[([](auto& c) { _val(c) = _attr(c); })] |
  ("-" >> function_call)[([](auto& c) { _val(c) = -_attr(c); })] |
  ("(" >> numeric_expression >> ")")[([](auto& c) { _val(c) = _attr(c); })] |
  ("-(" >> numeric_expression >> ")")[([](auto& c) { _val(c) = -_attr(c); })];
//------------------------------------------------------------------------------
struct numeric_factor_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<numeric_factor_tag, parsed_number_t> numeric_factor =
  "numeric-factor";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto numeric_factor_def = (coersion >> *('^' >> coersion))[([](auto& c) {
    auto l{at_c<0>(_attr(c))};
    const auto& ve{at_c<1>(_attr(c))};
    if(ve.empty()) {
        _val(c) = l;
    } else {
        parsed_number_t e{1};
        for(auto i = ve.rbegin(); i != ve.rend(); ++i) {
            e = std::pow(*i, e);
        }
        _val(c) = std::pow(l, e);
    }
})];
//------------------------------------------------------------------------------
struct numeric_term_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<numeric_term_tag, parsed_number_t> numeric_term = "numeric-term";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto numeric_term_def =
  (numeric_factor >> *(bs::char_("*/") >> numeric_factor))[([](auto& c) {
      auto l{at_c<0>(_attr(c))};
      for(const auto& p : at_c<1>(_attr(c))) {
          const auto o{at_c<0>(p)};
          const auto r{at_c<1>(p)};
          l = (o == '*') ? (l * r) : (l / r);
      }
      _val(c) = l;
  })];
//------------------------------------------------------------------------------
struct arith_expression_tag;
// NOLINTNEXTLINE(cert-err58-cpp)
const bs::rule<arith_expression_tag, parsed_number_t> arith_expression =
  "arithmetic-expression";
// NOLINTNEXTLINE(cert-err58-cpp)
const auto arith_expression_def =
  (numeric_term >> *(bs::char_("+-") >> numeric_term))[([](auto& c) {
      auto l{at_c<0>(_attr(c))};
      for(const auto& p : at_c<1>(_attr(c))) {
          const auto o{at_c<0>(p)};
          const auto r{at_c<1>(p)};
          l = (o == '+') ? (l + r) : (l - r);
      }
      _val(c) = l;
  })];
//------------------------------------------------------------------------------
// NOLINTNEXTLINE(cert-err58-cpp)
const auto function_call_def =
  (bs::lit("sqrt") >> "(" >> numeric_expression >>
   ")")[([](auto& c) { _val(c) = std::sqrt(_attr(c)); })] |
  (bs::lit("sin") >> "(" >> numeric_expression >>
   ")")[([](auto& c) { _val(c) = std::sin(_attr(c)); })] |
  (bs::lit("cos") >> "(" >> numeric_expression >>
   ")")[([](auto& c) { _val(c) = std::cos(_attr(c)); })] |
  (bs::lit("tg") >> "(" >> numeric_expression >>
   ")")[([](auto& c) { _val(c) = std::tan(_attr(c)); })] |
  (bs::lit("ln") >> "(" >> numeric_expression >>
   ")")[([](auto& c) { _val(c) = std::log(_attr(c)); })] |
  (bs::lit("exp") >> "(" >> numeric_expression >>
   ")")[([](auto& c) { _val(c) = std::exp(_attr(c)); })] |
  (bs::lit("e") >> "^" >>
   numeric_term)[([](auto& c) { _val(c) = std::exp(_attr(c)); })];
//------------------------------------------------------------------------------
// NOLINTNEXTLINE(cert-err58-cpp)
const auto numeric_expression_def = arith_expression | function_call;

// NOLINTNEXTLINE(modernize-use-trailing-return-type)
BOOST_SPIRIT_DEFINE(
  numeric_literal,
  juxtaposition,
  numeric_constant,
  conversion,
  coersion,
  numeric_factor,
  numeric_term,
  arith_expression,
  function_call,
  numeric_expression)
//------------------------------------------------------------------------------
template <typename Iter>
static inline auto do_parse(Iter i, Iter e, parsed_number_t& n) {
    if(bs::phrase_parse(i, e, numeric_expression, bs::ascii::space, n)) {
        return true;
    }
    return false;
}
//------------------------------------------------------------------------------
} // namespace numexpr
//------------------------------------------------------------------------------
EAGINE_LIB_FUNC
auto _parse_from_string(string_view src, long double& parsed) noexcept -> bool {
    numexpr::parsed_number_t temp{};
    // Spirit does not support non-ASCII7 characters
    // NOLINTNEXTLINE(hicpp-signed-bitwise)
    if(memory::all_of(src, [](char c) { return (c & ~(0x7f * 2 + 1)) == 0; })) {
        if(numexpr::do_parse(src.begin(), src.end(), temp)) {
            parsed = temp.real();
            return true;
        }
    }
    return false;
}
//------------------------------------------------------------------------------
EAGINE_LIB_FUNC
auto _parse_from_string(string_view src, long long int& parsed) noexcept
  -> bool {
    long double temp{};
    if(_parse_from_string(src, temp)) {
        const auto rnd{std::round(temp)};
        const auto eps{std::numeric_limits<long double>::epsilon()};
        if(std::abs(temp - rnd) <= eps) {
            parsed = static_cast<long long int>(rnd);
            return true;
        }
    }
    return false;
}
//------------------------------------------------------------------------------
#else
//------------------------------------------------------------------------------
EAGINE_LIB_FUNC
auto _parse_from_string(string_view, long double&) noexcept -> bool {
    return false;
}
//------------------------------------------------------------------------------
EAGINE_LIB_FUNC
auto _parse_from_string(string_view, long long int&) noexcept -> bool {
    return false;
}
//------------------------------------------------------------------------------
#endif
} // namespace eagine


/* Copyright 2017-2018 by Yifei Zheng
 *
 * 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.
 *
 * Unordered set support as well as set algorithms.
 */

#ifndef SET_SUPPORT_HPP
#define SET_SUPPORT_HPP
#include <unordered_set>
#include <boost/functional/hash.hpp>

template <typename T, class hash = boost::hash<T>>
using CSF_set = std::unordered_set<T, hash>;

template <class C1, class C2>
bool is_subset(const C1& S1, const C2& S2) {
    for (const auto& i : S1) {
        if (!S2.count(i))
            return false;
    }
    return true;
}

template <class C1, class C2>
C1 union_sets(const C1& S1, const C2& S2) {
    C1 ret = S1;
    for (auto& i: S2)
        ret.insert(i);
    return ret;
}

template <class C1, class C2>
C1 intersect_sets(const C1& S1, const C2& S2) {
    C1 ret;
    for (const auto& i: S2)
        if (S1.count(i))
            ret.insert(i);
    return ret;
}

template <class C1, class C2>
void exclude(C1& S1, const C2& S2) noexcept {
    for (const auto& i: S2)
        S1.erase(i);
}

template <class C, class T>
bool verify_then_remove(C& set, const T& key) {
    auto it = set.find(key);
    if (it != set.end()) {
        set.erase(it);
        return true;
    }
    return false;
}

template <class C, class T>
bool verify_then_insert(C& set, T key) {
    return set.insert(std::move(key)).second;
}
#endif


///////////////////////////////////////////////////////////////////////////////
// acvf_analysis.hpp                                                         //
//                                                                           //
//  Copyright 2008 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_ACCUMULATORS_STATISTICS_ACVF_ANALYSIS_HPP_ER_2008_04
#define BOOST_ACCUMULATORS_STATISTICS_ACVF_ANALYSIS_HPP_ER_2008_04
#include <iostream>
#include <algorithm>
#include <iterator>
#include <functional>
#include <stdexcept>
//#include <boost/assert.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/parameter/parameters.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/bind.hpp>
#include <boost/range.hpp>
#include <boost/ref.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range/iterator_range.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/delay.hpp>
#include <boost/accumulators/statistics/acvf_moving_average.hpp>
#include <boost/accumulators/statistics/acvf.hpp>
#include <boost/accumulators/statistics/acf.hpp>
#include <boost/accumulators/statistics/percentage_effective_sample_size.hpp>
#include <boost/accumulators/statistics/standard_error_autocorrelated.hpp>
#include <boost/accumulators/statistics/standard_error_iid.hpp>

namespace boost{namespace accumulators{
namespace statistics{

    template<
        typename RealType,
        typename I = default_delay_discriminator
    >
    class acvf_analysis{
        typedef I discr_t;
        public:
        typedef RealType    value_type;

        private:

        typedef boost::accumulators::accumulator_set<
        value_type, boost::accumulators::stats<
            boost::accumulators::tag::mean,
            boost::accumulators::tag::acf<discr_t>,
            boost::accumulators::tag::integrated_acvf<discr_t>,
            boost::accumulators::tag::percentage_effective_sample_size<
                discr_t>,
            boost::accumulators::tag::standard_error_autocorrelated<discr_t>,
            boost::accumulators::tag::standard_error_iid<discr_t>
            >
        >  acc_type;

    public://TODO define copy and assign
        acvf_analysis(std::size_t max_lag)
        :K(max_lag),
        acc(boost::accumulators::tag::delay<discr_t>::cache_size=(K+1)){};

        void operator()(value_type x){
            return acc(x);
        }

        template<typename R>
        void operator()(
            const R& range,
            std::size_t offset,
            std::size_t stride){

            // an iterator range?
            typedef typename range_iterator<const R>::type const_iter_type;
            typedef typename range_size<R>::type size_type;
            const_iter_type i = boost::begin(range);
            const_iter_type e = boost::end(range);
            if(std::distance(i,e)>offset){
                std::advance(i,offset);
                //this has the effect of rounding to smallest
                std::size_t d = (std::distance(i,e)-1)/stride;
                d *= stride;
                e = i; std::advance(e,d+1);

                while(i<e){
                    acc(*i);
                    std::advance(i,stride);
                }
//            for_each(
//                boost::begin(range),
//                boost::end(range),
//                boost::bind<void>(boost::ref(acc),_1)
//            );
            }else{
                std::runtime_error("acvf_analysis");
            }
        }
        std::size_t max_lag()const{ return K; }
        value_type mean()const{ return accumulators::mean(acc); }
        value_type standard_error_iid()const{
            return accumulators::standard_error_iid<discr_t>(acc);
        }
        value_type standard_error_autocorrelated()const{
            return accumulators
                ::standard_error_autocorrelated<discr_t>(acc);
        }
        value_type integrated_acvf()const{
            return accumulators
                ::integrated_acvf<discr_t>(acc);
        }

        value_type percentage_effective_sample_size()const{
            return accumulators
                ::percentage_effective_sample_size<discr_t>(acc);
        }

        void print(std::ostream& os)const
        {
            //using namespace boost::accumulators;
            os  << "count : " << accumulators::count(acc)
                << "\nacf : ";
                    copy(
                        begin(accumulators::acf<discr_t>(acc)),
                        end(accumulators::acf<discr_t>(acc)),
                        std::ostream_iterator<value_type>(os," ")
                    );
            os  << "\nintegrated_acvf : "
                << integrated_acvf()
                << "\ness% : "
                << percentage_effective_sample_size()
                << "\nmean : " << mean()
                << "\nstandard error : "
                << "\n assuming iid : "
                << standard_error_iid()
                << "\n assuming acf is zero after lag "
                << max_lag() << ": "
                << standard_error_autocorrelated() << std::endl;
        };

    private:

        std::size_t K;
        acc_type acc;
    };

//TODO
//    std::ostream& operator<<(std::ostream& os,const acvf_analysis& a){
//        a.print(os);
//        return os;
//    };

}
}}
#endif


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/ratio.hpp>

#include <boost/hana/assert.hpp>

#include <test/cnumeric.hpp>

#include <ratio>
using namespace boost::hana;

int main() {
    //////////////////////////////////////////////////////////////////////////
    // Conversions
    //////////////////////////////////////////////////////////////////////////
    {
        // Constant -> Ratio
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                to<ext::std::Ratio>(test::cnumeric<int, 0>),
                std::ratio<0>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                to<ext::std::Ratio>(test::cnumeric<int, 1>),
                std::ratio<1>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                to<ext::std::Ratio>(test::cnumeric<int, 3>),
                std::ratio<3>{}
            ));
        }
    }
}


#include "pow.hpp"
#include "utils.hpp"

#include <array>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/beast/core/detail/base64.hpp>
#include <iomanip>
#include <limits>
#include <openssl/sha.h>
#include <sstream>
#include <string.h>

using namespace std::chrono_literals;

constexpr int BYTE_LEN = 8;
constexpr std::chrono::milliseconds TIMESTAMP_VARIANCE = 15min;

using uint64Bytes = std::array<uint8_t, BYTE_LEN>;

// This enforces that the result array has the most significant byte at index 0
void u64ToU8Array(uint64_t numberVal, uint64Bytes& result) {
    for (int idx = result.size() - 1; idx >= 0; idx--) {
        // Grab the least significant byte
        result[idx] = numberVal & (uint64_t)0xFF;
        // Bitshift right one byte
        numberVal >>= BYTE_LEN;
    }
}

bool addWillOverflow(uint64_t x, uint64_t add) {
    return std::numeric_limits<uint64_t>::max() - x < add;
}

bool multWillOverflow(uint64_t left, uint64_t right) {
    return left != 0 &&
           (std::numeric_limits<std::uint64_t>::max() / left < right);
}

bool calcTarget(const std::string& payload, const uint64_t ttlInt,
                const int difficulty, uint64Bytes& target) {
    bool overflow = addWillOverflow(payload.size(), BYTE_LEN);
    if (overflow)
        return false;
    uint64_t totalLen = payload.size() + BYTE_LEN;
    overflow = multWillOverflow(ttlInt, totalLen);
    if (overflow)
        return false;
    uint64_t ttlMult = ttlInt * totalLen;
    uint64_t innerFrac = ttlMult / std::numeric_limits<uint16_t>::max();
    overflow = addWillOverflow(totalLen, innerFrac);
    if (overflow)
        return false;
    uint64_t lenPlusInnerFrac = totalLen + innerFrac;
    overflow = multWillOverflow(difficulty, lenPlusInnerFrac);
    if (overflow)
        return false;
    uint64_t denominator = difficulty * lenPlusInnerFrac;
    uint64_t targetNum = std::numeric_limits<uint64_t>::max() / denominator;

    u64ToU8Array(targetNum, target);
    return true;
}

int get_valid_difficulty(const std::string& timestamp,
                         const std::vector<pow_difficulty_t>& history) {
    uint64_t timestamp_long;
    try {
        timestamp_long = std::stoull(timestamp);
    } catch (...) {
        // Should never happen, checked previously
        return false;
    }
    const auto msg_timestamp = std::chrono::milliseconds(timestamp_long);

    int difficulty = std::numeric_limits<int>::max();
    int most_recent_difficulty = std::numeric_limits<int>::max();
    std::chrono::milliseconds most_recent(0);
    const std::chrono::milliseconds lower = msg_timestamp - TIMESTAMP_VARIANCE;
    const std::chrono::milliseconds upper = msg_timestamp + TIMESTAMP_VARIANCE;

    for (const auto& this_difficulty : history) {
        const std::chrono::milliseconds t = this_difficulty.timestamp;
        if (t < msg_timestamp && t >= most_recent) {
            most_recent = t;
            most_recent_difficulty = this_difficulty.difficulty;
        }

        if (t >= lower && t <= upper) {
            difficulty = std::min(this_difficulty.difficulty, difficulty);
        }
    }
    return std::min(most_recent_difficulty, difficulty);
}

bool checkPoW(const std::string& nonce, const std::string& timestamp,
              const std::string& ttl, const std::string& recipient,
              const std::string& data, std::string& messageHash,
              const int difficulty) {
    std::string payload;
    payload.reserve(timestamp.size() + ttl.size() + recipient.size() +
                    data.size());
    payload += timestamp;
    payload += ttl;
    payload += recipient;
    payload += data;

    uint64_t ttlInt;
    if (!util::parseTTL(ttl, ttlInt))
        return false;
    // ttl is in milliseconds, but target calculation wants seconds
    ttlInt = ttlInt / 1000;
    uint64Bytes target;
    calcTarget(payload, ttlInt, difficulty, target);

    uint8_t hashResult[SHA512_DIGEST_LENGTH];
    // Initial hash
    SHA512((const unsigned char*)payload.data(), payload.size(), hashResult);
    // Convert nonce to binary
    std::string decodedNonce = boost::beast::detail::base64_decode(nonce);
    // Convert decoded nonce string into uint8_t vector. Will have length 8
    std::vector<uint8_t> innerPayload;
    innerPayload.reserve(decodedNonce.size() + SHA512_DIGEST_LENGTH);
    innerPayload.insert(std::end(innerPayload), std::begin(decodedNonce),
                        std::end(decodedNonce));
    innerPayload.insert(std::end(innerPayload), hashResult,
                        hashResult + SHA512_DIGEST_LENGTH);
    // Final hash
    SHA512(innerPayload.data(), innerPayload.size(), hashResult);
    std::stringstream ss;
    ss << std::hex << std::setfill('0');
    for (int i = 0; i < SHA512_DIGEST_LENGTH; i++)
        ss << std::setw(2) << static_cast<unsigned>(hashResult[i]);
    messageHash = ss.str();

    return memcmp(hashResult, target.data(), BYTE_LEN) < 0;
}


///////////////////////////////////////////////////////////////////////////////
// moment.hpp
//
//  Copyright 2005 Eric Niebler. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_MOMENT_HPP_EAN_15_11_2005
#define BOOST_ACCUMULATORS_STATISTICS_MOMENT_HPP_EAN_15_11_2005

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>

namespace boost { namespace numeric
{
    /// INTERNAL ONLY
    ///
    template<typename T>
    T const &pow(T const &x, mpl::int_<1>)
    {
        return x;
    }

    /// INTERNAL ONLY
    ///
    template<typename T, int N>
    T pow(T const &x, mpl::int_<N>)
    {
        using namespace operators;
        T y = numeric::pow(x, mpl::int_<N/2>());
        T z = y * y;
        return (N % 2) ? (z * x) : z;
    }
}}

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // moment_impl
    template<typename N, typename Sample>
    struct moment_impl
      : accumulator_base // TODO: also depends_on sum of powers
    {
        BOOST_MPL_ASSERT_RELATION(N::value, >, 0);
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type result_type;

        template<typename Args>
        moment_impl(Args const &args)
          : sum(args[sample | Sample()])
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            this->sum += numeric::pow(args[sample], N());
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::fdiv(this->sum, count(args));
        }

    private:
        Sample sum;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::moment
//
namespace tag
{
    template<int N>
    struct moment
      : depends_on<count>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::moment_impl<mpl::int_<N>, mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::moment
//
namespace extract
{
    BOOST_ACCUMULATORS_DEFINE_EXTRACTOR(tag, moment, (int))
}

using extract::moment;

// So that moment<N> can be automatically substituted with
// weighted_moment<N> when the weight parameter is non-void
template<int N>
struct as_weighted_feature<tag::moment<N> >
{
    typedef tag::weighted_moment<N> type;
};

template<int N>
struct feature_of<tag::weighted_moment<N> >
  : feature_of<tag::moment<N> >
{
};

}} // namespace boost::accumulators

#endif


// Copyright (C) 2022 Chris Richardson and Athena Elafrou
// SPDX-License-Identifier:    MIT

#include <basix/e-lagrange.h>
#include <boost/program_options.hpp>
#include <cmath>
#include <dolfinx.h>
#include <dolfinx/common/Timer.h>
#include <dolfinx/common/log.h>
#include <dolfinx/io/XDMFFile.h>
#include <iostream>
#include <memory>

#include <cuda_profiler_api.h>
#include <nvToolsExt.h>

// Helper functions
#include <cuda/allocator.hpp>
#include <cuda/la.hpp>
#include <cuda/utils.hpp>

#include "VectorUpdater.hpp"

using namespace dolfinx;
namespace po = boost::program_options;

int main(int argc, char* argv[])
{
  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "size", po::value<std::size_t>()->default_value(32))(
      "degree", po::value<int>()->default_value(1));

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv)
                .options(desc)
                .allow_unregistered()
                .run(),
            vm);
  po::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << "\n";
    return 0;
  }

  const std::size_t Nx = vm["size"].as<std::size_t>();
  const int degree = vm["degree"].as<int>();

  //  common::subsystem::init_logging(argc, argv);
  common::subsystem::init_mpi(argc, argv);

  {
    // MPI
    MPI_Comm mpi_comm{MPI_COMM_WORLD};
    MPI_Comm local_comm;
    MPI_Comm_split_type(mpi_comm, MPI_COMM_TYPE_SHARED, 0, MPI_INFO_NULL,
                        &local_comm);

    int gpu_rank = utils::set_device(local_comm);
    int mpi_rank = dolfinx::MPI::rank(mpi_comm);
    MPI_Comm_free(&local_comm);

    std::string thread_name = "MPI: " + std::to_string(mpi_rank);
    loguru::set_thread_name(thread_name.c_str());
    MPI_Datatype data_type = dolfinx::MPI::mpi_type<double>();
    LOG(INFO) << "GPU rank=" << gpu_rank;

    // Read mesh and mesh tags
    std::array<std::array<double, 3>, 2> p
        = {{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}}};
    std::array<std::size_t, 3> n = {Nx, Nx, Nx};
    auto mesh = std::make_shared<mesh::Mesh>(mesh::create_box(
        mpi_comm, p, n, mesh::CellType::hexahedron, mesh::GhostMode::none));

    // Create a Basix continuous Lagrange element of given degree
    basix::FiniteElement e = basix::element::create_lagrange(
        mesh::cell_type_to_basix_type(mesh::CellType::hexahedron), degree,
        basix::element::lagrange_variant::equispaced, false);

    // Create a scalar function space
    auto V = std::make_shared<fem::FunctionSpace>(
        fem::create_functionspace(mesh, e, 1));
    auto idxmap = V->dofmap()->index_map;

    // Start profiling
    cudaProfilerStart();

    // Assemble RHS vector
    LOG(INFO) << "Allocate vector";
    CUDA::allocator<double> allocator{};
    la::Vector<double, decltype(allocator)> x(idxmap, 1, allocator);

    // Fill with rank values
    x.set((double)mpi_rank);

    VectorUpdater vu(x);

    nvtxMarkA("prefetch");
    // Prefetch data to gpu
    linalg::prefetch(gpu_rank, x);

    nvtxMarkA("update_fwd");
    for (int i = 0; i < 50; ++i)
      vu.update_fwd(x);

    nvtxMarkA("scatter_fwd");
    for (int i = 0; i < 50; ++i)
      x.scatter_fwd();

    nvtxMarkA("update_rev");
    for (int i = 0; i < 50; ++i)
      vu.update_rev(x);

    nvtxMarkA("scatter_rev");
    for (int i = 0; i < 50; ++i)
      x.scatter_rev(dolfinx::common::IndexMap::Mode::add);

    nvtxMarkA("end");
    // End profiling
    cudaProfilerStop();

    common::subsystem::finalize_mpi();
    return 0;

    // Now some timings

    dolfinx::common::Timer tcuda("Fwd CUDA-MPI");
    LOG(INFO) << "CUDA MPI updates";
    {
      for (int i = 0; i < 10000; ++i)
        vu.update_fwd(x);
    }
    tcuda.stop();

    dolfinx::common::Timer tcpu("Fwd CPU-MPI");
    LOG(INFO) << "CPU MPI updates";
    {
      for (int i = 0; i < 10000; ++i)
        x.scatter_fwd();
    }
    tcpu.stop();

    dolfinx::common::Timer tcuda2("Rev CUDA-MPI");
    LOG(INFO) << "CUDA MPI rev updates";
    {
      for (int i = 0; i < 10000; ++i)
        vu.update_rev(x);
    }
    tcuda2.stop();

    dolfinx::common::Timer tcpu2("Rev CPU-MPI");
    LOG(INFO) << "CPU MPI rev updates";
    {
      for (int i = 0; i < 10000; ++i)
        x.scatter_rev(dolfinx::common::IndexMap::Mode::add);
    }
    tcpu2.stop();
  }

  dolfinx::list_timings(MPI_COMM_WORLD, {dolfinx::TimingType::wall});

  common::subsystem::finalize_mpi();
  return 0;
}


/*
 * GridMapMathTest.cpp
 *
 *  Created on: Feb 10, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#include "grid_map_lib/GridMapMath.hpp"

// Eigen
#include <Eigen/Core>

// gtest
#include <gtest/gtest.h>

// Limits
#include <cfloat>

// Vector
#include <vector>

using namespace std;
using namespace Eigen;
using namespace grid_map_lib;

TEST(PositionFromIndex, Simple)
{
  Array2d mapLength(3.0, 2.0);
  Vector2d mapPosition(-1.0, 2.0);
  double resolution = 1.0;
  Array2i bufferSize(3, 2);
  Vector2d position;

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(0, 0), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(1.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(1, 0), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.5 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(1, 1), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(2, 1), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_DOUBLE_EQ(-1.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.5 + mapPosition.y(), position.y());

  EXPECT_FALSE(getPositionFromIndex(position, Array2i(3, 1), mapLength, mapPosition, resolution, bufferSize));
}

TEST(PositionFromIndex, CircularBuffer)
{
  Array2d mapLength(0.5, 0.4);
  Vector2d mapPosition(-0.1, 13.4);
  double resolution = 0.1;
  Array2i bufferSize(5, 4);
  Array2i bufferStartIndex(3, 1);
  Vector2d position;

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(3, 1), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.2 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.15 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(4, 2), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(0.05 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(2, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(-0.2 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(0, 0), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.0 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.15 + mapPosition.y(), position.y());

  EXPECT_TRUE(getPositionFromIndex(position, Array2i(4, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_DOUBLE_EQ(0.1 + mapPosition.x(), position.x());
  EXPECT_DOUBLE_EQ(-0.05 + mapPosition.y(), position.y());

  EXPECT_FALSE(getPositionFromIndex(position, Array2i(5, 3), mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
}

TEST(IndexFromPosition, Simple)
{
  Array2d mapLength(3.0, 2.0);
  Vector2d mapPosition(-12.4, -7.1);
  double resolution = 1.0;
  Array2i bufferSize(3, 2);
  Array2i index;

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(1.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(-1.0, -0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(2, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.6, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.4, -0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.4, 0.1) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_FALSE(getIndexFromPosition(index, Vector2d(4.0, 0.5) + mapPosition, mapLength, mapPosition, resolution, bufferSize));
}

TEST(IndexFromPosition, EdgeCases)
{
  Array2d mapLength(3.0, 2.0);
  Vector2d mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Array2i bufferSize(3, 2);
  Array2i index;

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.0, DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(0, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(1, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(-0.5 - DBL_EPSILON, -DBL_EPSILON), mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(2, index(0));
  EXPECT_EQ(1, index(1));
}

TEST(IndexFromPosition, CircularBuffer)
{
  Array2d mapLength(0.5, 0.4);
  Vector2d mapPosition(0.4, -0.9);
  double resolution = 0.1;
  Array2i bufferSize(5, 4);
  Array2i bufferStartIndex(3, 1);
  Array2i index;

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.2, 0.15) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index(0));
  EXPECT_EQ(1, index(1));

  EXPECT_TRUE(getIndexFromPosition(index, Vector2d(0.03, -0.17) + mapPosition, mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index(0));
  EXPECT_EQ(0, index(1));
}

TEST(checkIfPositionWithinMap, Inside)
{
  Array2d mapLength(50.0, 25.0);
  Vector2d mapPosition(11.4, 0.0);

  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(0.0, 0.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(5.0, 5.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(20.0, 10.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(20.0, -10.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(-20.0, 10.0) + mapPosition, mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(-20.0, -10.0) + mapPosition, mapLength, mapPosition));
}

TEST(checkIfPositionWithinMap, Outside)
{
  Array2d mapLength(10.0, 5.0);
  Vector2d mapPosition(-3.0, 145.2);

  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(5.5, 0.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(-5.5, 0.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(-5.5, 3.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(-5.5, -3.0) + mapPosition, mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(3.0, 3.0) + mapPosition, mapLength, mapPosition));
}

TEST(checkIfPositionWithinMap, EdgeCases)
{
  Array2d mapLength(2.0, 3.0);
  Vector2d mapPosition(0.0, 0.0);

  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(1.0, -1.5), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(-1.0, 1.5), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(1.0 + DBL_EPSILON, 1.0), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d((2.0 + DBL_EPSILON) / 2.0, 1.0), mapLength, mapPosition));
  EXPECT_FALSE(checkIfPositionWithinMap(Vector2d(0.5, -1.5 - (2.0 * DBL_EPSILON)), mapLength, mapPosition));
  EXPECT_TRUE(checkIfPositionWithinMap(Vector2d(-0.5, (3.0 + DBL_EPSILON) / 2.0), mapLength, mapPosition));
}

TEST(getIndexShiftFromPositionShift, All)
{
  double resolution = 1.0;
  Array2i indexShift;

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector2d(0.0, 0.0), resolution));
  EXPECT_EQ(0, indexShift(0));
  EXPECT_EQ(0, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector2d(0.35, -0.45), resolution));
  EXPECT_EQ(0, indexShift(0));
  EXPECT_EQ(0, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector2d(0.55, -0.45), resolution));
  EXPECT_EQ(-1, indexShift(0));
  EXPECT_EQ(0, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector2d(-1.3, -2.65), resolution));
  EXPECT_EQ(1, indexShift(0));
  EXPECT_EQ(3, indexShift(1));

  EXPECT_TRUE(getIndexShiftFromPositionShift(indexShift, Vector2d(-0.4, 0.09), 0.2));
  EXPECT_EQ(2, indexShift(0));
  EXPECT_EQ(0, indexShift(1));
}

TEST(getPositionShiftFromIndexShift, All)
{
  double resolution = 0.3;
  Vector2d positionShift;

  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Array2i(0, 0), resolution));
  EXPECT_DOUBLE_EQ(0.0, positionShift.x());
  EXPECT_DOUBLE_EQ(0.0, positionShift.y());

  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Array2i(1, -1), resolution));
  EXPECT_DOUBLE_EQ(-0.3, positionShift.x());
  EXPECT_DOUBLE_EQ(0.3, positionShift.y());

  EXPECT_TRUE(getPositionShiftFromIndexShift(positionShift, Array2i(2, 1), resolution));
  EXPECT_DOUBLE_EQ(-0.6, positionShift.x());
  EXPECT_DOUBLE_EQ(-0.3, positionShift.y());
}

TEST(checkIfIndexWithinRange, All)
{
  Array2i bufferSize(10, 15);
  EXPECT_TRUE(checkIfIndexWithinRange(Array2i(0, 0), bufferSize));
  EXPECT_TRUE(checkIfIndexWithinRange(Array2i(9, 14), bufferSize));
  EXPECT_FALSE(checkIfIndexWithinRange(Array2i(10, 5), bufferSize));
  EXPECT_FALSE(checkIfIndexWithinRange(Array2i(5, 300), bufferSize));
  EXPECT_FALSE(checkIfIndexWithinRange(Array2i(-1, 0), bufferSize));
  EXPECT_FALSE(checkIfIndexWithinRange(Array2i(0, -300), bufferSize));
}

TEST(mapIndexWithinRange, All)
{
  int index;
  int bufferSize = 10;

  index = 0;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(0, index);

  index = 1;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = -1;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = 9;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(9, index);

  index = 10;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(0, index);

  index = 11;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = 35;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(5, index);

  index = -9;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(1, index);

  index = -19;
  mapIndexWithinRange(index, bufferSize);
  EXPECT_EQ(1, index);
}

TEST(limitPositionToRange, Simple)
{
  double epsilon = 11.0 * numeric_limits<double>::epsilon();

  Array2d mapLength(30.0, 10.0);
  Vector2d mapPosition(0.0, 0.0);
  Vector2d position;

  position << 0.0, 0.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_DOUBLE_EQ(0.0, position.x());
  EXPECT_DOUBLE_EQ(0.0, position.y());

  position << 15.0, 5.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(15.0, position.x(), 15.0 * epsilon);
  EXPECT_GE(15.0, position.x());
  EXPECT_NEAR(5.0, position.y(), 5.0 * epsilon);
  EXPECT_GE(5.0, position.y());

  position << -15.0, -5.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-15.0, position.x(), 15.0 * epsilon);
  EXPECT_LE(-15.0, position.x());
  EXPECT_NEAR(-5.0, position.y(), 5.0 * epsilon);
  EXPECT_LE(-5.0, position.y());

  position << 16.0, 6.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(15.0, position.x(), 16.0 * epsilon);
  EXPECT_GE(15.0, position.x());
  EXPECT_NEAR(5.0, position.y(), 6.0 * epsilon);
  EXPECT_GE(5.0, position.y());

  position << -16.0, -6.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-15.0, position.x(), 16.0 * epsilon);
  EXPECT_LE(-15.0, position.x());
  EXPECT_NEAR(-5.0, position.y(), 6.0 * epsilon);
  EXPECT_LE(-5.0, position.y());

  position << 1e6, 1e6;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(15.0, position.x(), 1e6 * epsilon);
  EXPECT_GE(15.0, position.x());
  EXPECT_NEAR(5.0, position.y(), 1e6 * epsilon);
  EXPECT_GE(5.0, position.y());

  position << -1e6, -1e6;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-15.0, position.x(), 1e6 * epsilon);
  EXPECT_LE(-15.0, position.x());
  EXPECT_NEAR(-5.0, position.y(), 1e6 * epsilon);
  EXPECT_LE(-5.0, position.y());
}

TEST(limitPositionToRange, Position)
{
  double epsilon = 11.0 * numeric_limits<double>::epsilon();

  Array2d mapLength(30.0, 10.0);
  Vector2d mapPosition(1.0, 2.0);
  Vector2d position;

  position << 0.0, 0.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_DOUBLE_EQ(0.0, position.x());
  EXPECT_DOUBLE_EQ(0.0, position.y());

  position << 16.0, 7.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(16.0, position.x(), 16.0 * epsilon);
  EXPECT_GE(16.0, position.x());
  EXPECT_NEAR(7.0, position.y(), 7.0 * epsilon);
  EXPECT_GE(7.0, position.y());

  position << -14.0, -3.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-14.0, position.x(), 14.0 * epsilon);
  EXPECT_LE(-14.0, position.x());
  EXPECT_NEAR(-3.0, position.y(), 3.0 * epsilon);
  EXPECT_LE(-3.0, position.y());

  position << 17.0, 8.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(16.0, position.x(), 17.0 * epsilon);
  EXPECT_GE(16.0, position.x());
  EXPECT_NEAR(7.0, position.y(), 8.0 * epsilon);
  EXPECT_GE(7.0, position.y());

  position << -15.0, -4.0;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-14.0, position.x(), 15.0 * epsilon);
  EXPECT_LE(-14.0, position.x());
  EXPECT_NEAR(-3.0, position.y(), 4.0 * epsilon);
  EXPECT_LE(-3.0, position.y());

  position << 1e6, 1e6;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(16.0, position.x(), 1e6 * epsilon);
  EXPECT_GE(16.0, position.x());
  EXPECT_NEAR(7.0, position.y(), 1e6 * epsilon);
  EXPECT_GE(7.0, position.y());

  position << -1e6, -1e6;
  limitPositionToRange(position, mapLength, mapPosition);
  EXPECT_NEAR(-14.0, position.x(), 1e6 * epsilon);
  EXPECT_LE(-14.0, position.x());
  EXPECT_NEAR(-3.0, position.y(), 1e6 * epsilon);
  EXPECT_LE(-3.0, position.y());
}

TEST(getSubmapInformation, Simple)
{
  // Map
  Array2d mapLength(5.0, 4.0);
  Vector2d mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Array2i bufferSize(5, 4);

  // Requested submap
  Vector2d requestedSubmapPosition;
  Vector2d requestedSubmapLength;

  // The returned submap indeces
  Array2i submapTopLeftIndex;
  Array2i submapSize;
  Eigen::Vector2d submapPosition;
  Eigen::Array2d submapLength;
  Eigen::Array2i requestedIndexInSubmap;

  requestedSubmapPosition << 0.0, 0.5;
  requestedSubmapLength << 0.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                           requestedSubmapPosition, requestedSubmapLength, mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(2, submapTopLeftIndex(0));
  EXPECT_EQ(0, submapTopLeftIndex(1));
  EXPECT_EQ(1, submapSize(0));
  EXPECT_EQ(3, submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(1, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, Zero)
{
  // Map
  Array2d mapLength(5.0, 4.0);
  Vector2d mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Array2i bufferSize(5, 4);

  // Requested submap
  Vector2d requestedSubmapPosition;
  Vector2d requestedSubmapLength;

  // The returned submap indeces
  Array2i submapTopLeftIndex;
  Array2i submapSize;
  Eigen::Vector2d submapPosition;
  Eigen::Array2d submapLength;
  Eigen::Array2i requestedIndexInSubmap;

  requestedSubmapPosition << -1.0, -0.5;
  requestedSubmapLength << 0.0, 0.0;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(3, submapTopLeftIndex(0));
  EXPECT_EQ(2, submapTopLeftIndex(1));
  EXPECT_EQ(1, submapSize(0));
  EXPECT_EQ(1, submapSize(1));
  EXPECT_DOUBLE_EQ(requestedSubmapPosition.x(), submapPosition.x());
  EXPECT_DOUBLE_EQ(requestedSubmapPosition.y(), submapPosition.y());
  EXPECT_DOUBLE_EQ(resolution, submapLength(0));
  EXPECT_DOUBLE_EQ(resolution, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(0, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, ExceedingBoundaries)
{
  // Map
  Array2d mapLength(5.0, 4.0);
  Vector2d mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Array2i bufferSize(5, 4);

  // Requested submap
  Vector2d requestedSubmapPosition;
  Vector2d requestedSubmapLength;

  // The returned submap indeces
  Array2i submapTopLeftIndex;
  Array2i submapSize;
  Eigen::Vector2d submapPosition;
  Eigen::Array2d submapLength;
  Eigen::Array2i requestedIndexInSubmap;

  requestedSubmapPosition << 2.0, 1.5;
  requestedSubmapLength << 2.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, submapTopLeftIndex(0));
  EXPECT_EQ(0, submapTopLeftIndex(1));
  EXPECT_EQ(2, submapSize(0));
  EXPECT_EQ(2, submapSize(1));
  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(0, requestedIndexInSubmap(1));

  requestedSubmapPosition << 0.0, 0.0;
  requestedSubmapLength << 1e6, 1e6;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize));
  EXPECT_EQ(0, submapTopLeftIndex(0));
  EXPECT_EQ(0, submapTopLeftIndex(1));
  EXPECT_EQ(bufferSize(0), submapSize(0));
  EXPECT_EQ(bufferSize(1), submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
  EXPECT_EQ(2, requestedIndexInSubmap(0));
  EXPECT_LE(1, requestedIndexInSubmap(1));
  EXPECT_GE(2, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, CircularBuffer)
{
  // Map
  Array2d mapLength(5.0, 4.0);
  Vector2d mapPosition(0.0, 0.0);
  double resolution = 1.0;
  Array2i bufferSize(5, 4);
  Array2i bufferStartIndex(2, 1);

  // Requested submap
  Vector2d requestedSubmapPosition;
  Vector2d requestedSubmapLength;

  // The returned submap indeces
  Array2i submapTopLeftIndex;
  Array2i submapSize;
  Eigen::Vector2d submapPosition;
  Eigen::Array2d submapLength;
  Eigen::Array2i requestedIndexInSubmap;

  requestedSubmapPosition << 0.0, 0.5;
  requestedSubmapLength << 0.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(4, submapTopLeftIndex(0));
  EXPECT_EQ(1, submapTopLeftIndex(1));
  EXPECT_EQ(1, submapSize(0));
  EXPECT_EQ(3, submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.5, submapPosition.y());
  EXPECT_DOUBLE_EQ(1.0, submapLength(0));
  EXPECT_DOUBLE_EQ(3.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(1, requestedIndexInSubmap(1));

  requestedSubmapPosition << 2.0, 1.5;
  requestedSubmapLength << 2.9, 2.9;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, submapTopLeftIndex(0));
  EXPECT_EQ(1, submapTopLeftIndex(1));
  EXPECT_EQ(2, submapSize(0));
  EXPECT_EQ(2, submapSize(1));
  EXPECT_DOUBLE_EQ(1.5, submapPosition.x());
  EXPECT_DOUBLE_EQ(1.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(2.0, submapLength(0));
  EXPECT_DOUBLE_EQ(2.0, submapLength(1));
  EXPECT_EQ(0, requestedIndexInSubmap(0));
  EXPECT_EQ(0, requestedIndexInSubmap(1));

  requestedSubmapPosition << 0.0, 0.0;
  requestedSubmapLength << 1e6, 1e6;
  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, submapTopLeftIndex(0));
  EXPECT_EQ(1, submapTopLeftIndex(1));
  EXPECT_EQ(bufferSize(0), submapSize(0));
  EXPECT_EQ(bufferSize(1), submapSize(1));
  EXPECT_DOUBLE_EQ(0.0, submapPosition.x());
  EXPECT_DOUBLE_EQ(0.0, submapPosition.y());
  EXPECT_DOUBLE_EQ(mapLength(0), submapLength(0));
  EXPECT_DOUBLE_EQ(mapLength(1), submapLength(1));
  EXPECT_EQ(2, requestedIndexInSubmap(0));
  EXPECT_LE(1, requestedIndexInSubmap(1));
  EXPECT_GE(2, requestedIndexInSubmap(1));
}

TEST(getSubmapInformation, Debug1)
{
  // Map
  Array2d mapLength(4.98, 4.98);
  Vector2d mapPosition(-4.98, -5.76);
  double resolution = 0.06;
  Array2i bufferSize(83, 83);
  Array2i bufferStartIndex(0, 13);

  // Requested submap
  Vector2d requestedSubmapPosition(-7.44, -3.42);
  Vector2d requestedSubmapLength(0.12, 0.12);

  // The returned submap indeces
  Array2i submapTopLeftIndex;
  Array2i submapSize;
  Eigen::Vector2d submapPosition;
  Eigen::Array2d submapLength;
  Eigen::Array2i requestedIndexInSubmap;

  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, submapSize(0));
  EXPECT_EQ(3, submapSize(1));
  EXPECT_DOUBLE_EQ(0.12, submapLength(0));
  EXPECT_DOUBLE_EQ(0.18, submapLength(1));
}

TEST(getSubmapInformation, Debug2)
{
  // Map
  Array2d mapLength(4.98, 4.98);
  Vector2d mapPosition(2.46, -25.26);
  double resolution = 0.06;
  Array2i bufferSize(83, 83);
  Array2i bufferStartIndex(42, 6);

  // Requested submap
  Vector2d requestedSubmapPosition(0.24, -26.82);
  Vector2d requestedSubmapLength(0.624614, 0.462276);

  // The returned submap indeces
  Array2i submapTopLeftIndex;
  Array2i submapSize;
  Eigen::Vector2d submapPosition;
  Eigen::Array2d submapLength;
  Eigen::Array2i requestedIndexInSubmap;

  EXPECT_TRUE(getSubmapInformation(submapTopLeftIndex, submapSize, submapPosition, submapLength, requestedIndexInSubmap,
                                   requestedSubmapPosition, requestedSubmapLength,
                                    mapLength, mapPosition, resolution, bufferSize, bufferStartIndex));
  EXPECT_LT(0, submapSize(0));
  EXPECT_LT(0, submapSize(1));
  EXPECT_LT(0.0, submapLength(0));
  EXPECT_LT(0.0, submapLength(1));
}

TEST(getBufferRegionsForSubmap, Trivial)
{
  unsigned int topLeft = bufferRegionIndeces[BufferRegion::TopLeft];
  unsigned int topRight = bufferRegionIndeces[BufferRegion::TopRight];
  unsigned int bottomLeft= bufferRegionIndeces[BufferRegion::BottomLeft];
  unsigned int bottomRight= bufferRegionIndeces[BufferRegion::BottomRight];

  Eigen::Array2i bufferSize(5, 4);
  vector<Eigen::Array2i> submapIndeces, submapSizes;

  Eigen::Array2i submapIndex(0, 0);
  Eigen::Array2i submapSize(0, 0);
  EXPECT_TRUE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize));
  EXPECT_EQ(0, submapSizes[topLeft][0]);
  EXPECT_EQ(0, submapSizes[topLeft][1]);
  EXPECT_EQ(0, submapSizes[topRight][0]);
  EXPECT_EQ(0, submapSizes[topRight][1]);
  EXPECT_EQ(0, submapSizes[bottomLeft][0]);
  EXPECT_EQ(0, submapSizes[bottomLeft][1]);
  EXPECT_EQ(0, submapSizes[bottomRight][0]);
  EXPECT_EQ(0, submapSizes[bottomRight][1]);

  submapSize << 0, 7;
  EXPECT_FALSE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize));

  submapSize << 6, 7;
  EXPECT_FALSE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize));
}

TEST(getBufferRegionsForSubmap, Simple)
{
  unsigned int topLeft = bufferRegionIndeces[BufferRegion::TopLeft];
  unsigned int topRight = bufferRegionIndeces[BufferRegion::TopRight];
  unsigned int bottomLeft= bufferRegionIndeces[BufferRegion::BottomLeft];
  unsigned int bottomRight= bufferRegionIndeces[BufferRegion::BottomRight];

  Eigen::Array2i submapIndex;
  Eigen::Array2i submapSize;
  Eigen::Array2i bufferSize(5, 4);
  vector<Eigen::Array2i> submapIndeces, submapSizes;

  submapIndex << 1, 2;
  submapSize << 3, 2;
  EXPECT_TRUE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize));
  EXPECT_EQ(1, submapIndeces[topLeft][0]);
  EXPECT_EQ(2, submapIndeces[topLeft][1]);
  EXPECT_EQ(3, submapSizes[topLeft][0]);
  EXPECT_EQ(2, submapSizes[topLeft][1]);
  EXPECT_EQ(0, submapSizes[topRight][0]);
  EXPECT_EQ(0, submapSizes[topRight][1]);
  EXPECT_EQ(0, submapSizes[bottomLeft][0]);
  EXPECT_EQ(0, submapSizes[bottomLeft][1]);
  EXPECT_EQ(0, submapSizes[bottomRight][0]);
  EXPECT_EQ(0, submapSizes[bottomRight][1]);
}

TEST(getBufferRegionsForSubmap, CircularBuffer)
{
  unsigned int topLeft = bufferRegionIndeces[BufferRegion::TopLeft];
  unsigned int topRight = bufferRegionIndeces[BufferRegion::TopRight];
  unsigned int bottomLeft= bufferRegionIndeces[BufferRegion::BottomLeft];
  unsigned int bottomRight= bufferRegionIndeces[BufferRegion::BottomRight];

  Eigen::Array2i submapIndex;
  Eigen::Array2i submapSize;
  Eigen::Array2i bufferSize(5, 4);
  Eigen::Array2i bufferStartIndex(3, 1);
  vector<Eigen::Array2i> submapIndeces, submapSizes;

  submapIndex << 3, 1;
  submapSize << 2, 3;
  EXPECT_TRUE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, submapIndeces[topLeft][0]);
  EXPECT_EQ(1, submapIndeces[topLeft][1]);
  EXPECT_EQ(2, submapSizes[topLeft][0]);
  EXPECT_EQ(3, submapSizes[topLeft][1]);
  EXPECT_EQ(0, submapSizes[topRight][0]);
  EXPECT_EQ(0, submapSizes[topRight][1]);
  EXPECT_EQ(0, submapSizes[bottomLeft][0]);
  EXPECT_EQ(0, submapSizes[bottomLeft][1]);
  EXPECT_EQ(0, submapSizes[bottomRight][0]);
  EXPECT_EQ(0, submapSizes[bottomRight][1]);

  submapIndex << 4, 1;
  submapSize << 2, 3;
  getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize, bufferStartIndex);
  EXPECT_EQ(4, submapIndeces[topLeft][0]);
  EXPECT_EQ(1, submapIndeces[topLeft][1]);
  EXPECT_EQ(1, submapSizes[topLeft][0]);
  EXPECT_EQ(3, submapSizes[topLeft][1]);
  EXPECT_EQ(0, submapSizes[topRight][0]);
  EXPECT_EQ(0, submapSizes[topRight][1]);
  EXPECT_EQ(0, submapIndeces[bottomLeft][0]);
  EXPECT_EQ(1, submapIndeces[bottomLeft][1]);
  EXPECT_EQ(1, submapSizes[bottomLeft][0]);
  EXPECT_EQ(3, submapSizes[bottomLeft][1]);
  EXPECT_EQ(0, submapSizes[bottomRight][0]);
  EXPECT_EQ(0, submapSizes[bottomRight][1]);

  submapIndex << 1, 0;
  submapSize << 2, 1;
  EXPECT_TRUE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapSizes[topLeft][0]);
  EXPECT_EQ(0, submapSizes[topLeft][1]);
  EXPECT_EQ(0, submapSizes[topRight][0]);
  EXPECT_EQ(0, submapSizes[topRight][1]);
  EXPECT_EQ(0, submapSizes[bottomLeft][0]);
  EXPECT_EQ(0, submapSizes[bottomLeft][1]);
  EXPECT_EQ(1, submapIndeces[bottomRight][0]);
  EXPECT_EQ(0, submapIndeces[bottomRight][1]);
  EXPECT_EQ(2, submapSizes[bottomRight][0]);
  EXPECT_EQ(1, submapSizes[bottomRight][1]);

  submapIndex << 3, 1;
  submapSize << 5, 4;
  EXPECT_TRUE(getBufferRegionsForSubmap(submapIndeces, submapSizes, submapIndex, submapSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, submapIndeces[topLeft][0]);
  EXPECT_EQ(1, submapIndeces[topLeft][1]);
  EXPECT_EQ(2, submapSizes[topLeft][0]);
  EXPECT_EQ(3, submapSizes[topLeft][1]);
  EXPECT_EQ(3, submapIndeces[topRight][0]);
  EXPECT_EQ(0, submapIndeces[topRight][1]);
  EXPECT_EQ(2, submapSizes[topRight][0]);
  EXPECT_EQ(1, submapSizes[topRight][1]);
  EXPECT_EQ(0, submapIndeces[bottomLeft][0]);
  EXPECT_EQ(1, submapIndeces[bottomLeft][1]);
  EXPECT_EQ(3, submapSizes[bottomLeft][0]);
  EXPECT_EQ(3, submapSizes[bottomLeft][1]);
  EXPECT_EQ(0, submapIndeces[bottomRight][0]);
  EXPECT_EQ(0, submapIndeces[bottomRight][1]);
  EXPECT_EQ(3, submapSizes[bottomRight][0]);
  EXPECT_EQ(1, submapSizes[bottomRight][1]);
}

TEST(checkIncrementIndex, Simple)
{
  Eigen::Array2i index(0, 0);
  Eigen::Array2i bufferSize(4, 3);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(1, index[1]);

  for (int i = 0; i < 6; i++) {
    EXPECT_TRUE(incrementIndex(index, bufferSize));
  }
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_FALSE(incrementIndex(index, bufferSize));
  EXPECT_EQ(index[0], index[0]);
  EXPECT_EQ(index[1], index[1]);
}

TEST(checkIncrementIndex, CircularBuffer)
{
  Eigen::Array2i bufferSize(4, 3);
  Eigen::Array2i bufferStartIndex(2, 1);
  Eigen::Array2i index(bufferStartIndex);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(2, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_FALSE(incrementIndex(index, bufferSize, bufferStartIndex));
  EXPECT_EQ(index[0], index[0]);
  EXPECT_EQ(index[1], index[1]);
}

TEST(checkIncrementIndexForSubmap, Simple)
{
  Eigen::Array2i submapIndex(0, 0);
  Eigen::Array2i index;
  Eigen::Array2i submapTopLeftIndex(3, 1);
  Eigen::Array2i submapBufferSize(2, 4);
  Eigen::Array2i bufferSize(8, 5);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(1, submapIndex[1]);
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(2, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(2, submapIndex[1]);
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(3, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(3, index[0]);
  EXPECT_EQ(4, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(0, submapIndex[1]);
  EXPECT_EQ(4, index[0]);
  EXPECT_EQ(1, index[1]);

  submapIndex << 1, 2;
  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(4, index[0]);
  EXPECT_EQ(4, index[1]);

  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));

  submapIndex << 2, 0;
  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize));
}

TEST(checkIncrementIndexForSubmap, CircularBuffer)
{
  Eigen::Array2i submapIndex(0, 0);
  Eigen::Array2i index;
  Eigen::Array2i submapTopLeftIndex(6, 3);
  Eigen::Array2i submapBufferSize(2, 4);
  Eigen::Array2i bufferSize(8, 5);
  Eigen::Array2i bufferStartIndex(3, 2);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(1, submapIndex[1]);
  EXPECT_EQ(6, index[0]);
  EXPECT_EQ(4, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(2, submapIndex[1]);
  EXPECT_EQ(6, index[0]);
  EXPECT_EQ(0, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(0, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(6, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(0, submapIndex[1]);
  EXPECT_EQ(7, index[0]);
  EXPECT_EQ(3, index[1]);

  submapIndex << 1, 2;
  EXPECT_TRUE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
  EXPECT_EQ(1, submapIndex[0]);
  EXPECT_EQ(3, submapIndex[1]);
  EXPECT_EQ(7, index[0]);
  EXPECT_EQ(1, index[1]);

  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));

  submapIndex << 2, 0;
  EXPECT_FALSE(incrementIndexForSubmap(submapIndex, index, submapTopLeftIndex, submapBufferSize, bufferSize, bufferStartIndex));
}