#ifndef STAN_MATH_TORSTEN_ONECPT_RK45_HPP
#define STAN_MATH_TORSTEN_ONECPT_RK45_HPP

#include <Eigen/Dense>
#include <stan/math/torsten/to_array_2d.hpp>
#include <stan/math/torsten/ev_manager.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <stan/math/torsten/pmx_coupled_model.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/dsolve/pmx_odeint_integrator.hpp>
#include <vector>
#include <string>
#include <stan/math/torsten/ev_solver.hpp>

namespace torsten {

/**
 * Compute the predicted amounts in each compartment at each event
 * of an ODEs model. The model contains a base 1 Compartment PK
 * component which gets solved analytically, while the other ODEs
 * are solved numerically using stan::math::integrate_ode_rk45. This
 * amounts to using the mixed solver method.
 *
 * <b>Warning:</b> This prototype does not handle steady state events.
 *
 * @tparam T0 type of scalar for time of events.
 * @tparam T1 type of scalar for amount at each event.
 * @tparam T2 type of scalar for rate at each event.
 * @tparam T3 type of scalar for inter-dose inteveral at each event.
 * @tparam T4 type of scalars for the model parameters.
 * @tparam T5 type of scalars for the bio-variability parameters.
 * @tparam T6 type of scalars for the model tlag parameters.
 * @tparam F type of ODE system function.
 * @param[in] f functor for base ordinary differential equation
 *            which gets solved numerically.
 * @param[in] nOde number of ODEs we solve numerically.
 * @param[in] time times of events
 * @param[in] amt amount at each event
 * @param[in] rate rate at each event
 * @param[in] ii inter-dose interval at each event
 * @param[in] evid event identity:
 *                    (0) observation
 *                    (1) dosing
 *                    (2) other
 *                    (3) reset
 *                    (4) reset AND dosing
 * @param[in] cmt compartment number at each event
 * @param[in] addl additional dosing at each event
 * @param[in] ss steady state approximation at each event (0: no, 1: yes)
 * @param[in] theta vector of ODE parameters
 * @param[in] biovar bio-availability in each compartment
 * @param[in] tlag lag time in each compartment
 * @param[in] rel_tol relative tolerance for the Boost ode solver
 * @param[in] abs_tol absolute tolerance for the Boost ode solver
 * @param[in] max_num_steps maximal number of steps to take within
 *            the Boost ode solver
 * @return a matrix with predicted amount in each compartment
 *         at each event.
 *
 * FIX ME: msg should be passed on to functor (allows use of
 * print statement inside ODE system).
 */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename stan::return_type_t<T0, T1, T2, T3, T4, T5, T6>,
               Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_onecpt_rk45(const F& f,
                      const int nOde,
                      const std::vector<T0>& time,
                      const std::vector<T1>& amt,
                      const std::vector<T2>& rate,
                      const std::vector<T3>& ii,
                      const std::vector<int>& evid,
                      const std::vector<int>& cmt,
                      const std::vector<int>& addl,
                      const std::vector<int>& ss,
                      const std::vector<std::vector<T4> >& theta,
                      const std::vector<std::vector<T5> >& biovar,
                      const std::vector<std::vector<T6> >& tlag,
                     double rel_tol,
                     double abs_tol,
                     long int max_num_steps,
                     double as_rel_tol,
                     double as_abs_tol,
                     long int as_max_num_steps,
                     std::ostream* msgs = 0) {
  using std::vector;
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using boost::math::tools::promote_args;
  using torsten::dsolve::odeint_scheme_rk45;

  // check arguments
  static const char* function("pmx_solve_onecpt_rk45");
  torsten::pmx_check(time, amt, rate, ii, evid, cmt, addl, ss, theta, function);

  // Construct dummy array of matrix for last argument of pred
  Matrix<T4, Dynamic, Dynamic> dummy_system;
  vector<Matrix<T4, Dynamic, Dynamic> > dummy_systems(1, dummy_system);

  const int &nPK = torsten::PMXOneCptModel<double>::Ncmt;

  dsolve::PMXOdeIntegrator<dsolve::PMXVariadicOdeSystem, dsolve::PMXOdeintIntegrator<odeint_scheme_rk45>>
    integrator(rel_tol, abs_tol, max_num_steps, as_rel_tol, as_abs_tol, as_max_num_steps, msgs);
  const int nCmt = nPK + nOde;

  using ER = NONMENEventsRecord<T0, T1, T2, T3>;
  using EM = EventsManager<ER, NonEventParameters<T0, T4, std::vector, std::tuple<T5, T6> >>;
  const ER events_rec(nCmt, time, amt, rate, ii, evid, cmt, addl, ss);

  Matrix<typename EM::T_scalar, Dynamic, Dynamic> pred =
    Matrix<typename EM::T_scalar, Dynamic, Dynamic>::Zero(events_rec.num_event_times(), EM::nCmt(events_rec));

  using model_type = torsten::PkOneCptOdeModel<typename EM::T_par, F>;
  EventSolver<model_type, EM> pr;
  pr.pred(0, events_rec, pred, integrator, theta, biovar, tlag, nOde, f);
  return pred;
}

  /*
   * overload with default ode & algebra solver controls
   */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename stan::return_type_t<T0, T1, T2, T3, T4, T5, T6>,
               Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_onecpt_rk45(const F& f,
                      const int nOde,
                      const std::vector<T0>& time,
                      const std::vector<T1>& amt,
                      const std::vector<T2>& rate,
                      const std::vector<T3>& ii,
                      const std::vector<int>& evid,
                      const std::vector<int>& cmt,
                      const std::vector<int>& addl,
                      const std::vector<int>& ss,
                      const std::vector<std::vector<T4> >& theta,
                      const std::vector<std::vector<T5> >& biovar,
                      const std::vector<std::vector<T6> >& tlag,
                      std::ostream* msgs = 0) {
  return pmx_solve_onecpt_rk45(f, nOde,
                               time, amt, rate, ii, evid, cmt, addl, ss,
                               theta, biovar, tlag,
                               1.e-6, 1.e-6, 1e6,
                               1.e-6, 1.e-6, 1e2,
                               msgs);
}

  /*
   * overload with default algebra solver controls
   */
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
Eigen::Matrix <typename stan::return_type_t<T0, T1, T2, T3, T4, T5, T6>,
               Eigen::Dynamic, Eigen::Dynamic>
pmx_solve_onecpt_rk45(const F& f,
                      const int nOde,
                      const std::vector<T0>& time,
                      const std::vector<T1>& amt,
                      const std::vector<T2>& rate,
                      const std::vector<T3>& ii,
                      const std::vector<int>& evid,
                      const std::vector<int>& cmt,
                      const std::vector<int>& addl,
                      const std::vector<int>& ss,
                      const std::vector<std::vector<T4> >& theta,
                      const std::vector<std::vector<T5> >& biovar,
                      const std::vector<std::vector<T6> >& tlag,
                     double rel_tol,
                     double abs_tol,
                     long int max_num_steps,
                     std::ostream* msgs = 0) {
  return pmx_solve_onecpt_rk45(f, nOde,
                               time, amt, rate, ii, evid, cmt, addl, ss,
                               theta, biovar, tlag,
                               rel_tol, abs_tol, max_num_steps,
                               1.e-6, 1.e-6, 1e2,
                               msgs);
}

  /**
   * Overload function to allow user to pass an std::vector for
   * pMatrix/bioavailability/tlag
   */
  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename F,
            typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  pmx_solve_onecpt_rk45(const F& f,
                        const int nOde,
                        const std::vector<T0>& time,
                        const std::vector<T1>& amt,
                        const std::vector<T2>& rate,
                        const std::vector<T3>& ii,
                        const std::vector<int>& evid,
                        const std::vector<int>& cmt,
                        const std::vector<int>& addl,
                        const std::vector<int>& ss,
                        const std::vector<T_par>& pMatrix,
                        const std::vector<T_biovar>& biovar,
                        const std::vector<T_tlag>& tlag,
                       double rel_tol,
                       double abs_tol,
                       long int max_num_steps,
                       double as_rel_tol,
                       double as_abs_tol,
                       long int as_max_num_steps,
                       std::ostream* msgs = 0) {
    auto param_ = torsten::to_array_2d(pMatrix);
    auto biovar_ = torsten::to_array_2d(biovar);
    auto tlag_ = torsten::to_array_2d(tlag);

    return pmx_solve_onecpt_rk45(f, nOde,
                                 time, amt, rate, ii, evid, cmt, addl, ss,
                                 param_, biovar_, tlag_,
                                 rel_tol, abs_tol, max_num_steps,
                                 as_rel_tol, as_abs_tol, as_max_num_steps,
                                 msgs);
  }

  /**
   * Overload function to allow user to pass an std::vector for
   * pMatrix/bioavailability/tlag, with default ode &
   * algebra solver controls
   */
  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename F,
            typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  pmx_solve_onecpt_rk45(const F& f,
                        const int nOde,
                        const std::vector<T0>& time,
                        const std::vector<T1>& amt,
                        const std::vector<T2>& rate,
                        const std::vector<T3>& ii,
                        const std::vector<int>& evid,
                        const std::vector<int>& cmt,
                        const std::vector<int>& addl,
                        const std::vector<int>& ss,
                        const std::vector<T_par>& pMatrix,
                        const std::vector<T_biovar>& biovar,
                        const std::vector<T_tlag>& tlag,
                        std::ostream* msgs = 0) {
    return pmx_solve_onecpt_rk45(f, nOde,
                                 time, amt, rate, ii, evid, cmt, addl, ss,
                                 pMatrix, biovar, tlag,
                                 1.e-6, 1.e-6, 1e6,
                                 1.e-6, 1.e-6, 1e2,
                                 msgs);
  }

  /**
   * Overload function to allow user to pass an std::vector for
   * pMatrix/bioavailability/tlag, with default
   * algebra solver controls
   */
  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename F,
            typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  pmx_solve_onecpt_rk45(const F& f,
                        const int nOde,
                        const std::vector<T0>& time,
                        const std::vector<T1>& amt,
                        const std::vector<T2>& rate,
                        const std::vector<T3>& ii,
                        const std::vector<int>& evid,
                        const std::vector<int>& cmt,
                        const std::vector<int>& addl,
                        const std::vector<int>& ss,
                        const std::vector<T_par>& pMatrix,
                        const std::vector<T_biovar>& biovar,
                        const std::vector<T_tlag>& tlag,
                        double rel_tol,
                        double abs_tol,
                        long int max_num_steps,
                        std::ostream* msgs = 0) {
    return pmx_solve_onecpt_rk45(f, nOde,
                                 time, amt, rate, ii, evid, cmt, addl, ss,
                                 pMatrix, biovar, tlag,
                                 rel_tol, abs_tol, max_num_steps,
                                 1.e-6, 1.e-6, 1e2,
                                 msgs);
  }

  // old version
template <typename T0, typename T1, typename T2, typename T3, typename T4,
          typename T5, typename T6, typename F>
stan::matrix_return_t<T0, T1, T2, T3, T4, T5, T6>
mixOde1CptModel_rk45(const F& f,
                     const int nOde,
                     const std::vector<T0>& time,
                     const std::vector<T1>& amt,
                     const std::vector<T2>& rate,
                     const std::vector<T3>& ii,
                     const std::vector<int>& evid,
                     const std::vector<int>& cmt,
                     const std::vector<int>& addl,
                     const std::vector<int>& ss,
                     const std::vector<std::vector<T4> >& theta,
                     const std::vector<std::vector<T5> >& biovar,
                     const std::vector<std::vector<T6> >& tlag,
                     double rel_tol = 1e-6,
                     double abs_tol = 1e-6,
                     long int max_num_steps = 1e6,
                     std::ostream* msgs = 0) {
  auto x = pmx_solve_onecpt_rk45(f, nOde,
                                 time, amt, rate, ii, evid, cmt, addl, ss,
                                 theta, biovar, tlag,
                                 rel_tol, abs_tol, max_num_steps,
                                 msgs);
  return x.transpose();
}

  template <typename T0, typename T1, typename T2, typename T3,
            typename T_par, typename T_biovar, typename T_tlag,
            typename F,
            typename = require_any_not_std_vector_t<T_par, T_biovar, T_tlag> >
  stan::matrix_return_t<T0, T1, T2, T3, T_par, T_biovar, T_tlag>
  mixOde1CptModel_rk45(const F& f,
                        const int nOde,
                        const std::vector<T0>& time,
                        const std::vector<T1>& amt,
                        const std::vector<T2>& rate,
                        const std::vector<T3>& ii,
                        const std::vector<int>& evid,
                        const std::vector<int>& cmt,
                        const std::vector<int>& addl,
                        const std::vector<int>& ss,
                        const std::vector<T_par>& pMatrix,
                        const std::vector<T_biovar>& biovar,
                        const std::vector<T_tlag>& tlag,
                        double rel_tol = 1e-6,
                        double abs_tol = 1e-6,
                       long int max_num_steps = 1e6,
                       std::ostream* msgs = 0) {
    auto x = pmx_solve_onecpt_rk45(f, nOde,
                                   time, amt, rate, ii, evid, cmt, addl, ss,
                                   pMatrix, biovar, tlag,
                                   rel_tol, abs_tol, max_num_steps,
                                   msgs);
    return x.transpose();
  }

}
#endif


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// Use, modification and distribution is subject to 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Purpose: Implementation of the HEALPix and rHEALPix projections.
//          For background see <http://code.scenzgrid.org/index.php/p/scenzgrid-py/source/tree/master/docs/rhealpix_dggs.pdf>.
// Authors: Alex Raichev (raichev@cs.auckland.ac.nz)
//          Michael Speth (spethm@landcareresearch.co.nz)
// Notes:   Raichev implemented these projections in Python and
//          Speth translated them into C here.

// Copyright (c) 2001, Thomas Flemming, tf@ttqv.com

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_HEALPIX_HPP
#define BOOST_GEOMETRY_PROJECTIONS_HEALPIX_HPP

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

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/pj_auth.hpp>
#include <boost/geometry/srs/projections/impl/pj_qsfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace healpix
    {

            /* Fuzz to handle rounding errors: */
            static const double epsilon = 1e-15;

            template <typename T>
            struct par_healpix
            {
                int north_square;
                int south_square;
                T qp;
                detail::apa<T> apa;
            };

            template <typename T>
            struct cap_map
            {
                int cn; /* An integer 0--3 indicating the position of the polar cap. */
                T x, y; /* Coordinates of the pole point (point of most extreme latitude on the polar caps). */
                enum region_type {north, south, equatorial} region;
            };
            template <typename T>
            struct point_xy
            {
                T x, y;
            };

            /* IDENT, R1, R2, R3, R1 inverse, R2 inverse, R3 inverse:*/
            static double rot[7][2][2] = {
                /* Identity matrix */
                {{1, 0},{0, 1}},
                /* Matrix for counterclockwise rotation by pi/2: */
                {{ 0,-1},{ 1, 0}},
                /* Matrix for counterclockwise rotation by pi: */
                {{-1, 0},{ 0,-1}},
                /* Matrix for counterclockwise rotation by 3*pi/2:  */
                {{ 0, 1},{-1, 0}},
                {{ 0, 1},{-1, 0}}, // 3*pi/2
                {{-1, 0},{ 0,-1}}, // pi
                {{ 0,-1},{ 1, 0}}  // pi/2
            };

            /**
             * Returns the sign of the double.
             * @param v the parameter whose sign is returned.
             * @return 1 for positive number, -1 for negative, and 0 for zero.
             **/
            template <typename T>
            inline T pj_sign (T const& v)
            {
                return v > 0 ? 1 : (v < 0 ? -1 : 0);
            }
            /**
             * Return the index of the matrix in {{{1, 0},{0, 1}}, {{ 0,-1},{ 1, 0}}, {{-1, 0},{ 0,-1}}, {{ 0, 1},{-1, 0}}, {{ 0, 1},{-1, 0}}, {{-1, 0},{ 0,-1}}, {{ 0,-1},{ 1, 0}}}.
             * @param index ranges from -3 to 3.
             */
            inline int get_rotate_index(int index)
            {
                switch(index) {
                case 0:
                    return 0;
                case 1:
                    return 1;
                case 2:
                    return 2;
                case 3:
                    return 3;
                case -1:
                    return 4;
                case -2:
                    return 5;
                case -3:
                    return 6;
                }
                return 0;
            }
            /**
             * Return 1 if point (testx, testy) lies in the interior of the polygon
             * determined by the vertices in vert, and return 0 otherwise.
             * See http://paulbourke.net/geometry/polygonmesh/ for more details.
             * @param nvert the number of vertices in the polygon.
             * @param vert the (x, y)-coordinates of the polygon's vertices
             **/
            template <typename T>
            inline int pnpoly(int nvert, T vert[][2], T const& testx, T const& testy)
            {
                int i;
                int counter = 0;
                T xinters;
                point_xy<T> p1, p2;

                /* Check for boundrary cases */
                for (i = 0; i < nvert; i++) {
                    if (testx == vert[i][0] && testy == vert[i][1]) {
                        return 1;
                    }
                }

                p1.x = vert[0][0];
                p1.y = vert[0][1];

                for (i = 1; i < nvert; i++) {
                    p2.x = vert[i % nvert][0];
                    p2.y = vert[i % nvert][1];
                    if (testy > (std::min)(p1.y, p2.y)  &&
                        testy <= (std::max)(p1.y, p2.y) &&
                        testx <= (std::max)(p1.x, p2.x) &&
                        p1.y != p2.y)
                    {
                        xinters = (testy-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x;
                        if (p1.x == p2.x || testx <= xinters)
                            counter++;
                    }
                    p1 = p2;
                }

                if (counter % 2 == 0) {
                    return 0;
                } else {
                    return 1;
                }
            }
            /**
             * Return 1 if (x, y) lies in (the interior or boundary of) the image of the
             * HEALPix projection (in case proj=0) or in the image the rHEALPix projection
             * (in case proj=1), and return 0 otherwise.
             * @param north_square the position of the north polar square (rHEALPix only)
             * @param south_square the position of the south polar square (rHEALPix only)
             **/
            template <typename T>
            inline int in_image(T const& x, T const& y, int proj, int north_square, int south_square)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                if (proj == 0) {
                    T healpixVertsJit[][2] = {
                        {-pi - epsilon,   fourth_pi},
                        {-3.0*fourth_pi,  half_pi + epsilon},
                        {-half_pi,        fourth_pi + epsilon},
                        {-fourth_pi,      half_pi + epsilon},
                        {0.0,             fourth_pi + epsilon},
                        {fourth_pi,       half_pi + epsilon},
                        {half_pi,         fourth_pi + epsilon},
                        {3.0*fourth_pi,   half_pi + epsilon},
                        {pi + epsilon,    fourth_pi},
                        {pi + epsilon,   -fourth_pi},
                        {3.0*fourth_pi,  -half_pi - epsilon},
                        {half_pi,        -fourth_pi - epsilon},
                        {fourth_pi,      -half_pi - epsilon},
                        {0.0,            -fourth_pi - epsilon},
                        {-fourth_pi,     -half_pi - epsilon},
                        {-half_pi,       -fourth_pi - epsilon},
                        {-3.0*fourth_pi, -half_pi - epsilon},
                        {-pi - epsilon,  -fourth_pi}
                    };
                    return pnpoly((int)sizeof(healpixVertsJit)/
                                  sizeof(healpixVertsJit[0]), healpixVertsJit, x, y);
                } else {
                    T rhealpixVertsJit[][2] = {
                        {-pi - epsilon,                                 fourth_pi + epsilon},
                        {-pi + north_square*half_pi - epsilon,          fourth_pi + epsilon},
                        {-pi + north_square*half_pi - epsilon,          3.0*fourth_pi + epsilon},
                        {-pi + (north_square + 1.0)*half_pi + epsilon,  3.0*fourth_pi + epsilon},
                        {-pi + (north_square + 1.0)*half_pi + epsilon,  fourth_pi + epsilon},
                        {pi + epsilon,                                  fourth_pi + epsilon},
                        {pi + epsilon,                                 -fourth_pi - epsilon},
                        {-pi + (south_square + 1.0)*half_pi + epsilon, -fourth_pi - epsilon},
                        {-pi + (south_square + 1.0)*half_pi + epsilon, -3.0*fourth_pi - epsilon},
                        {-pi + south_square*half_pi - epsilon,         -3.0*fourth_pi - epsilon},
                        {-pi + south_square*half_pi - epsilon,         -fourth_pi - epsilon},
                        {-pi - epsilon,                                -fourth_pi - epsilon}
                    };

                    return pnpoly((int)sizeof(rhealpixVertsJit)/
                                  sizeof(rhealpixVertsJit[0]), rhealpixVertsJit, x, y);
                }
            }
            /**
             * Return the authalic latitude of latitude alpha (if inverse=0) or
             * return the approximate latitude of authalic latitude alpha (if inverse=1).
             * P contains the relavent ellipsoid parameters.
             **/
            template <typename Parameters, typename T>
            inline T auth_lat(const Parameters& par, const par_healpix<T>& proj_parm, T const& alpha, int inverse)
            {
                if (inverse == 0) {
                    /* Authalic latitude. */
                    T q = pj_qsfn(sin(alpha), par.e, 1.0 - par.es);
                    T qp = proj_parm.qp;
                    T ratio = q/qp;

                    if (math::abs(ratio) > 1) {
                        /* Rounding error. */
                        ratio = pj_sign(ratio);
                    }

                    return asin(ratio);
                } else {
                    /* Approximation to inverse authalic latitude. */
                    return pj_authlat(alpha, proj_parm.apa);
                }
            }
            /**
             * Return the HEALPix projection of the longitude-latitude point lp on
             * the unit sphere.
            **/
            template <typename T>
            inline void healpix_sphere(T const& lp_lam, T const& lp_phi, T& xy_x, T& xy_y)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                T lam = lp_lam;
                T phi = lp_phi;
                T phi0 = asin(T(2.0)/T(3.0));

                /* equatorial region */
                if ( fabsl(phi) <= phi0) {
                    xy_x = lam;
                    xy_y = 3.0*pi/8.0*sin(phi);
                } else {
                    T lamc;
                    T sigma = sqrt(3.0*(1 - math::abs(sin(phi))));
                    T cn = floor(2*lam / pi + 2);
                    if (cn >= 4) {
                        cn = 3;
                    }
                    lamc = -3*fourth_pi + half_pi*cn;
                    xy_x = lamc + (lam - lamc)*sigma;
                    xy_y = pj_sign(phi)*fourth_pi*(2 - sigma);
                }
                return;
            }
            /**
             * Return the inverse of healpix_sphere().
            **/
            template <typename T>
            inline void healpix_sphere_inverse(T const& xy_x, T const& xy_y, T& lp_lam, T& lp_phi)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                T x = xy_x;
                T y = xy_y;
                T y0 = fourth_pi;

                /* Equatorial region. */
                if (math::abs(y) <= y0) {
                    lp_lam = x;
                    lp_phi = asin(8.0*y/(3.0*pi));
                } else if (fabsl(y) < half_pi) {
                    T cn = floor(2.0*x/pi + 2.0);
                    T xc, tau;
                    if (cn >= 4) {
                        cn = 3;
                    }
                    xc = -3.0*fourth_pi + (half_pi)*cn;
                    tau = 2.0 - 4.0*fabsl(y)/pi;
                    lp_lam = xc + (x - xc)/tau;
                    lp_phi = pj_sign(y)*asin(1.0 - math::pow(tau, 2)/3.0);
                } else {
                    lp_lam = -1.0*pi;
                    lp_phi = pj_sign(y)*half_pi;
                }
                return;
            }
            /**
             * Return the vector sum a + b, where a and b are 2-dimensional vectors.
             * @param ret holds a + b.
             **/
            template <typename T>
            inline void vector_add(const T a[2], const T b[2], T ret[2])
            {
                int i;
                for(i = 0; i < 2; i++) {
                    ret[i] = a[i] + b[i];
                }
            }
            /**
             * Return the vector difference a - b, where a and b are 2-dimensional vectors.
             * @param ret holds a - b.
             **/
            template <typename T>
            inline void vector_sub(const T a[2], const T b[2], T ret[2])
            {
                int i;
                for(i = 0; i < 2; i++) {
                    ret[i] = a[i] - b[i];
                }
            }
            /**
             * Return the 2 x 1 matrix product a*b, where a is a 2 x 2 matrix and
             * b is a 2 x 1 matrix.
             * @param ret holds a*b.
             **/
            template <typename T1, typename T2>
            inline void dot_product(const T1 a[2][2], const T2 b[2], T2 ret[2])
            {
                int i, j;
                int length = 2;
                for(i = 0; i < length; i++) {
                    ret[i] = 0;
                    for(j = 0; j < length; j++) {
                        ret[i] += a[i][j]*b[j];
                    }
                }
            }
            /**
             * Return the number of the polar cap, the pole point coordinates, and
             * the region that (x, y) lies in.
             * If inverse=0, then assume (x,y) lies in the image of the HEALPix
             * projection of the unit sphere.
             * If inverse=1, then assume (x,y) lies in the image of the
             * (north_square, south_square)-rHEALPix projection of the unit sphere.
             **/
            template <typename T>
            inline cap_map<T> get_cap(T x, T const& y, int north_square, int south_square,
                                     int inverse)
            {
                static const T pi = detail::pi<T>();
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                cap_map<T> capmap;
                T c;
                capmap.x = x;
                capmap.y = y;
                if (inverse == 0) {
                    if (y > fourth_pi) {
                        capmap.region = cap_map<T>::north;
                        c = half_pi;
                    } else if (y < -fourth_pi) {
                        capmap.region = cap_map<T>::south;
                        c = -half_pi;
                    } else {
                        capmap.region = cap_map<T>::equatorial;
                        capmap.cn = 0;
                        return capmap;
                    }
                    /* polar region */
                    if (x < -half_pi) {
                        capmap.cn = 0;
                        capmap.x = (-3.0*fourth_pi);
                        capmap.y = c;
                    } else if (x >= -half_pi && x < 0) {
                        capmap.cn = 1;
                        capmap.x = -fourth_pi;
                        capmap.y = c;
                    } else if (x >= 0 && x < half_pi) {
                        capmap.cn = 2;
                        capmap.x = fourth_pi;
                        capmap.y = c;
                    } else {
                        capmap.cn = 3;
                        capmap.x = 3.0*fourth_pi;
                        capmap.y = c;
                    }
                } else {
                    if (y > fourth_pi) {
                        capmap.region = cap_map<T>::north;
                        capmap.x = (-3.0*fourth_pi + north_square*half_pi);
                        capmap.y = half_pi;
                        x = x - north_square*half_pi;
                    } else if (y < -fourth_pi) {
                        capmap.region = cap_map<T>::south;
                        capmap.x = (-3.0*fourth_pi + south_square*pi/2);
                        capmap.y = -half_pi;
                        x = x - south_square*half_pi;
                    } else {
                        capmap.region = cap_map<T>::equatorial;
                        capmap.cn = 0;
                        return capmap;
                    }
                    /* Polar Region, find the HEALPix polar cap number that
                       x, y moves to when rHEALPix polar square is disassembled. */
                    if (capmap.region == cap_map<T>::north) {
                        if (y >= -x - fourth_pi - epsilon && y < x + 5.0*fourth_pi - epsilon) {
                            capmap.cn = (north_square + 1) % 4;
                        } else if (y > -x -fourth_pi + epsilon && y >= x + 5.0*fourth_pi - epsilon) {
                            capmap.cn = (north_square + 2) % 4;
                        } else if (y <= -x -fourth_pi + epsilon && y > x + 5.0*fourth_pi + epsilon) {
                            capmap.cn = (north_square + 3) % 4;
                        } else {
                            capmap.cn = north_square;
                        }
                    } else if (capmap.region == cap_map<T>::south) {
                        if (y <= x + fourth_pi + epsilon && y > -x - 5.0*fourth_pi + epsilon) {
                            capmap.cn = (south_square + 1) % 4;
                        } else if (y < x + fourth_pi - epsilon && y <= -x - 5.0*fourth_pi + epsilon) {
                            capmap.cn = (south_square + 2) % 4;
                        } else if (y >= x + fourth_pi - epsilon && y < -x - 5.0*fourth_pi - epsilon) {
                            capmap.cn = (south_square + 3) % 4;
                        } else {
                            capmap.cn = south_square;
                        }
                    }
                }
                return capmap;
            }
            /**
             * Rearrange point (x, y) in the HEALPix projection by
             * combining the polar caps into two polar squares.
             * Put the north polar square in position north_square and
             * the south polar square in position south_square.
             * If inverse=1, then uncombine the polar caps.
             * @param north_square integer between 0 and 3.
             * @param south_square integer between 0 and 3.
             **/
            template <typename T>
            inline void combine_caps(T& xy_x, T& xy_y, int north_square, int south_square,
                                     int inverse)
            {
                static const T half_pi = detail::half_pi<T>();
                static const T fourth_pi = detail::fourth_pi<T>();

                T v[2];
                T c[2];
                T vector[2];
                T v_min_c[2];
                T ret_dot[2];
                const double (*tmpRot)[2];
                int pole = 0;

                cap_map<T> capmap = get_cap(xy_x, xy_y, north_square, south_square, inverse);
                if (capmap.region == cap_map<T>::equatorial) {
                    xy_x = capmap.x;
                    xy_y = capmap.y;
                    return;
                }

                v[0] = xy_x; v[1] = xy_y;
                c[0] = capmap.x; c[1] = capmap.y;

                if (inverse == 0) {
                    /* Rotate (xy_x, xy_y) about its polar cap tip and then translate it to
                       north_square or south_square. */

                    if (capmap.region == cap_map<T>::north) {
                        pole = north_square;
                        tmpRot = rot[get_rotate_index(capmap.cn - pole)];
                    } else {
                        pole = south_square;
                        tmpRot = rot[get_rotate_index(-1*(capmap.cn - pole))];
                    }
                } else {
                    /* Inverse function.
                     Unrotate (xy_x, xy_y) and then translate it back. */

                    /* disassemble */
                    if (capmap.region == cap_map<T>::north) {
                        pole = north_square;
                        tmpRot = rot[get_rotate_index(-1*(capmap.cn - pole))];
                    } else {
                        pole = south_square;
                        tmpRot = rot[get_rotate_index(capmap.cn - pole)];
                    }
                }

                vector_sub(v, c, v_min_c);
                dot_product(tmpRot, v_min_c, ret_dot);

                {
                    T a[2];
                    /* Workaround cppcheck git issue */
                    T* pa = a;
                    // TODO: in proj4 5.0.0 this line is used instead
                    //pa[0] = -3.0*fourth_pi + ((inverse == 0) ? 0 : capmap.cn) *half_pi;
                    pa[0] = -3.0*fourth_pi + ((inverse == 0) ? pole : capmap.cn) *half_pi;
                    pa[1] = half_pi;
                    vector_add(ret_dot, a, vector);
                }

                xy_x = vector[0];
                xy_y = vector[1];
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_healpix_ellipsoid
                : public base_t_fi<base_healpix_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_healpix_ellipsoid(const Parameters& par)
                    : base_t_fi<base_healpix_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_healpix_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 0);
                    return healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(e_healpix_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the HEALPix image. */
                    if (in_image(xy_x, xy_y, 0, 0, 0) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 1);
                }

                static inline std::string get_name()
                {
                    return "healpix_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_healpix_spheroid
                : public base_t_fi<base_healpix_spheroid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_healpix_spheroid(const Parameters& par)
                    : base_t_fi<base_healpix_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_healpix_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    return healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(s_healpix_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the HEALPix image */
                    if (in_image(xy_x, xy_y, 0, 0, 0) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    return healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                }

                static inline std::string get_name()
                {
                    return "healpix_spheroid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_rhealpix_ellipsoid
                : public base_t_fi<base_rhealpix_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_rhealpix_ellipsoid(const Parameters& par)
                    : base_t_fi<base_rhealpix_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_rhealpix_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 0);
                    healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 0);
                }

                // INVERSE(e_rhealpix_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the rHEALPix image. */
                    if (in_image(xy_x, xy_y, 1, this->m_proj_parm.north_square, this->m_proj_parm.south_square) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 1);
                    healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                    lp_lat = auth_lat(this->params(), m_proj_parm, lp_lat, 1);
                }

                static inline std::string get_name()
                {
                    return "rhealpix_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_rhealpix_spheroid
                : public base_t_fi<base_rhealpix_spheroid<T, Parameters>, T, Parameters>
            {
                par_healpix<T> m_proj_parm;

                inline base_rhealpix_spheroid(const Parameters& par)
                    : base_t_fi<base_rhealpix_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_rhealpix_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    healpix_sphere(lp_lon, lp_lat, xy_x, xy_y);
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 0);
                }

                // INVERSE(s_rhealpix_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    /* Check whether (x, y) lies in the rHEALPix image. */
                    if (in_image(xy_x, xy_y, 1, this->m_proj_parm.north_square, this->m_proj_parm.south_square) == 0) {
                        lp_lon = HUGE_VAL;
                        lp_lat = HUGE_VAL;
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_x_or_y) );
                    }
                    combine_caps(xy_x, xy_y, this->m_proj_parm.north_square, this->m_proj_parm.south_square, 1);
                    return healpix_sphere_inverse(xy_x, xy_y, lp_lon, lp_lat);
                }

                static inline std::string get_name()
                {
                    return "rhealpix_spheroid";
                }

            };

            // HEALPix
            template <typename Parameters, typename T>
            inline void setup_healpix(Parameters& par, par_healpix<T>& proj_parm)
            {
                if (par.es != 0.0) {
                    proj_parm.apa = pj_authset<T>(par.es); /* For auth_lat(). */
                    proj_parm.qp = pj_qsfn(1.0, par.e, par.one_es); /* For auth_lat(). */
                    par.a = par.a*sqrt(0.5*proj_parm.qp); /* Set par.a to authalic radius. */
                    pj_calc_ellipsoid_params(par, par.a, par.es); /* Ensure we have a consistent parameter set */
                } else {
                }
            }

            // rHEALPix
            template <typename Params, typename Parameters, typename T>
            inline void setup_rhealpix(Params const& params, Parameters& par, par_healpix<T>& proj_parm)
            {
                proj_parm.north_square = pj_get_param_i<srs::spar::north_square>(params, "north_square", srs::dpar::north_square);
                proj_parm.south_square = pj_get_param_i<srs::spar::south_square>(params, "south_square", srs::dpar::south_square);
                /* Check for valid north_square and south_square inputs. */
                if ((proj_parm.north_square < 0) || (proj_parm.north_square > 3)) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_axis) );
                }
                if ((proj_parm.south_square < 0) || (proj_parm.south_square > 3)) {
                    BOOST_THROW_EXCEPTION( projection_exception(error_axis) );
                }
                if (par.es != 0.0) {
                    proj_parm.apa = pj_authset<T>(par.es); /* For auth_lat(). */
                    proj_parm.qp = pj_qsfn(1.0, par.e, par.one_es); /* For auth_lat(). */
                    par.a = par.a*sqrt(0.5*proj_parm.qp); /* Set par.a to authalic radius. */
                    // TODO: why not the same as in healpix?
                    //pj_calc_ellipsoid_params(par, par.a, par.es);
                    par.ra = 1.0/par.a;
                } else {
                }
            }

    }} // namespace detail::healpix
    #endif // doxygen

    /*!
        \brief HEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_healpix.gif
    */
    template <typename T, typename Parameters>
    struct healpix_ellipsoid : public detail::healpix::base_healpix_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline healpix_ellipsoid(Params const& , Parameters const& par)
            : detail::healpix::base_healpix_ellipsoid<T, Parameters>(par)
        {
            detail::healpix::setup_healpix(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief HEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_healpix.gif
    */
    template <typename T, typename Parameters>
    struct healpix_spheroid : public detail::healpix::base_healpix_spheroid<T, Parameters>
    {
        template <typename Params>
        inline healpix_spheroid(Params const& , Parameters const& par)
            : detail::healpix::base_healpix_spheroid<T, Parameters>(par)
        {
            detail::healpix::setup_healpix(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief rHEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - north_square (integer)
         - south_square (integer)
        \par Example
        \image html ex_rhealpix.gif
    */
    template <typename T, typename Parameters>
    struct rhealpix_ellipsoid : public detail::healpix::base_rhealpix_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline rhealpix_ellipsoid(Params const& params, Parameters const& par)
            : detail::healpix::base_rhealpix_ellipsoid<T, Parameters>(par)
        {
            detail::healpix::setup_rhealpix(params, this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief rHEALPix projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - north_square (integer)
         - south_square (integer)
        \par Example
        \image html ex_rhealpix.gif
    */
    template <typename T, typename Parameters>
    struct rhealpix_spheroid : public detail::healpix::base_rhealpix_spheroid<T, Parameters>
    {
        template <typename Params>
        inline rhealpix_spheroid(Params const& params, Parameters const& par)
            : detail::healpix::base_rhealpix_spheroid<T, Parameters>(par)
        {
            detail::healpix::setup_rhealpix(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_healpix, healpix_spheroid, healpix_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_rhealpix, rhealpix_spheroid, rhealpix_ellipsoid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(healpix_entry, healpix_spheroid, healpix_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(rhealpix_entry, rhealpix_spheroid, rhealpix_ellipsoid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(healpix_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(healpix, healpix_entry)
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(rhealpix, rhealpix_entry)
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_HEALPIX_HPP


#include <Rcpp.h>
#include <RcppEigen.h>
#include <vector>

#include "experimentalSetup.hpp"
#include "encodingScheme.hpp"
#include "individual.hpp"
#include "population.hpp"
#include "evolutionaryAlgorithm.hpp"
#include "AuxiliaryFunctions.hpp"
#include "logLikelihoods.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/normal.hpp>

EvolutionaryAlgorithm::EvolutionaryAlgorithm() { }

EvolutionaryAlgorithm::EvolutionaryAlgorithm(ExperimentalSetup & ES, const std::size_t & populationSize, InitialPopulationRandomVariates & RV)
{
    PopulationSize = populationSize;
    CurrentPopulation = InitialisePopulation(ES, RV);
}

EvolutionaryAlgorithm::EvolutionaryAlgorithm(ExperimentalSetup & ES, const std::size_t & populationSize,
                                             const std::size_t & numberOfIterations, const std::size_t & numberOfIterationsEqualMinMax,
                                             const std::size_t & numberOfFittestIndividuals,
                                             const int & parentSelectionWindowSize, const bool & allowParentSurvival,
                                             const double & crossoverProbability, const double & mutationProbabilityLowerLimit,
                                             const std::size_t & mutationIterations, const double & mutationDegreesOfFreedom,
                                             const Eigen::VectorXd & mutationDecay, const double & fractionFittestIndividuals,
                                             const std::size_t & hillClimbingIterations, InitialPopulationRandomVariates & RV)
{
    PopulationSize = populationSize;
    NumberOfFittestIndividuals = numberOfFittestIndividuals;
    NumberOfIterations = numberOfIterations;
    NumberOfIterationsEqualMinMax = numberOfIterationsEqualMinMax;

    ParentSelectionWindowSize = parentSelectionWindowSize;
    AllowParentSurvival = allowParentSurvival;

    CrossoverProbability = crossoverProbability;

    MutationProbabilityLowerLimit = mutationProbabilityLowerLimit;
    MutationIterations = mutationIterations;

    MutationDegreesOfFreedom = mutationDegreesOfFreedom;
    MutationDecay = mutationDecay;
    MutationDecay_t = MutationDecay[0];

    FittestEnsuredSurvivalFraction = fractionFittestIndividuals;
    FitnessEnsuredSurvivalFraction = -HUGE_VAL;

    HillClimbingIterations = hillClimbingIterations;

    CurrentPopulation = InitialisePopulation(ES, RV);
    Iteration = 0;

    // Finds maximum fitness index
    Eigen::VectorXd::Index fittestIndividualIndex;
    CurrentPopulation.Fitness.maxCoeff(&fittestIndividualIndex);
    FittestIndividualIndex = fittestIndividualIndex;
}

EvolutionaryAlgorithm::EvolutionaryAlgorithm(ExperimentalSetup & ES, Population & P,
                                             const std::size_t & numberOfIterations, const std::size_t & numberOfIterationsEqualMinMax,
                                             const std::size_t & numberOfFittestIndividuals,
                                             const int & parentSelectionWindowSize, const bool & allowParentSurvival,
                                             const double & crossoverProbability, const double & mutationProbabilityLowerLimit,
                                             const std::size_t & mutationIterations, const double & mutationDegreesOfFreedom,
                                             const Eigen::VectorXd & mutationDecay, const double & fractionFittestIndividuals,
                                             const std::size_t & hillClimbingIterations)
{
    CurrentPopulation = P;

    PopulationSize = P.Individuals.size();
    NumberOfFittestIndividuals = numberOfFittestIndividuals;
    NumberOfIterations = numberOfIterations;
    NumberOfIterationsEqualMinMax = numberOfIterationsEqualMinMax;

    ParentSelectionWindowSize = parentSelectionWindowSize;
    AllowParentSurvival = allowParentSurvival;

    CrossoverProbability = crossoverProbability;

    MutationProbabilityLowerLimit = mutationProbabilityLowerLimit;
    MutationIterations = mutationIterations;
    MutationDegreesOfFreedom = mutationDegreesOfFreedom;
    MutationDecay = mutationDecay;
    MutationDecay_t = MutationDecay[0];

    FittestEnsuredSurvivalFraction = fractionFittestIndividuals;
    FitnessEnsuredSurvivalFraction = -HUGE_VAL;

    HillClimbingIterations = hillClimbingIterations;
    Iteration = 0;

    // Finds maximum fitness index
    Eigen::VectorXd::Index fittestIndividualIndex;
    CurrentPopulation.Fitness.maxCoeff(&fittestIndividualIndex);
    FittestIndividualIndex = fittestIndividualIndex;
}

void EvolutionaryAlgorithm::RestructingIndividual(Individual & I, const ExperimentalSetup & ES)
{
    const std::size_t & NumberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;
    // Sorting unknown contributor mixtures
    const Eigen::VectorXd & Mixtures = I.MixtureParameters.segment(ES.NumberOfKnownContributors, NumberOfUnknownContributors);

    const std::vector<int> & sortedMixtures = sortedIndex(Mixtures);
    for (std::size_t n = 0; n < NumberOfUnknownContributors; n++)
    {
        I.MixtureParameters[ES.NumberOfKnownContributors + n] = Mixtures[sortedMixtures[n]];
    }

    // Sorting unknown genotypes
    const Eigen::VectorXd unsortedEncodedProfile = I.EncodedProfile;
    const std::vector<Eigen::MatrixXd> unsortedReducedContributionMatrix = I.ReducedExpectedContributionMatrix;
    for (std::size_t m = 0; m < ES.NumberOfMarkers; m++)
    {
        const Eigen::MatrixXd & unsortedReducedContributionMatrix_m = unsortedReducedContributionMatrix[m];
        for (std::size_t c = 0; c < NumberOfUnknownContributors; c++)
        {
            I.ReducedExpectedContributionMatrix[m].col(c) = unsortedReducedContributionMatrix_m.col(sortedMixtures[c]);

            std::size_t k = 2 * NumberOfUnknownContributors * m + 2 * c;
            std::size_t contributorElement = 2 * NumberOfUnknownContributors * m + 2 * sortedMixtures[c];

            if (unsortedEncodedProfile[contributorElement] <= unsortedEncodedProfile[contributorElement + 1]) {
                I.EncodedProfile[k] = unsortedEncodedProfile[contributorElement];
                I.EncodedProfile[k + 1] = unsortedEncodedProfile[contributorElement + 1];
            }
            else {
                I.EncodedProfile[k] = unsortedEncodedProfile[contributorElement + 1];
                I.EncodedProfile[k + 1] = unsortedEncodedProfile[contributorElement];
            }
        }
    }
}

Population EvolutionaryAlgorithm::InitialisePopulation(ExperimentalSetup & ES, InitialPopulationRandomVariates & RV)
{
    std::vector<Individual> I(PopulationSize);
    for (std::size_t n = 0; n < PopulationSize; n++)
    {
        Eigen::VectorXd U = ES.GenerateUnknownGenotype(RV);

        Individual I_n = Individual(U, ES);

        RestructingIndividual(I_n, ES);
        I[n] = I_n;
    }

    Population P(I);
    return P;
}

std::size_t EvolutionaryAlgorithm::ChoosePartner(const Population & P, int currentIndividual, RandomVariates & RV)
{
    int lowerWindow = std::round(std::max(0.0, static_cast<double>(currentIndividual - ParentSelectionWindowSize)));
    int upperWindow = std::round(std::min(currentIndividual + ParentSelectionWindowSize, static_cast<int>(PopulationSize - 1)));

    int leftBound = (currentIndividual - ParentSelectionWindowSize + PopulationSize) % PopulationSize;
    int rightBound = (currentIndividual + ParentSelectionWindowSize) % PopulationSize;

    Eigen::VectorXd neighbourhood = Eigen::VectorXd::Zero(2 * ParentSelectionWindowSize);
    Eigen::VectorXd logNeighbourhoodFitness = Eigen::VectorXd::Zero(2 * ParentSelectionWindowSize);
    for (std::size_t i = 0; i < 2 * ParentSelectionWindowSize; i++)
    {
        bool j = (i >= ParentSelectionWindowSize);
        int k = (leftBound + i + j) % PopulationSize;
        neighbourhood[i] = k;
        logNeighbourhoodFitness[i] = P.Fitness[k];
    }

    double logNeighbourhoodFitnessCenter = logNeighbourhoodFitness.maxCoeff();
    Eigen::VectorXd neighbourhoodFitness = Eigen::VectorXd::Zero(2 * ParentSelectionWindowSize);
    for (std::size_t i = 0; i < 2 * ParentSelectionWindowSize; i++)
    {
        neighbourhoodFitness[i] = std::exp(logNeighbourhoodFitness[i] - logNeighbourhoodFitnessCenter);
    }

    Eigen::VectorXd windowProbabilities = partialSumEigen(neighbourhoodFitness / neighbourhoodFitness.sum());

    double u = RV.generate_uniform_real();

    int h = 0;
    while ((u > windowProbabilities[h + 1])) {
        h++;
    }

    std::size_t partnerIndex = neighbourhood[h];
    return partnerIndex;
}

Individual EvolutionaryAlgorithm::Crossover(const Individual & I, const Individual & J, const ExperimentalSetup & ES, RandomVariates & RV)
{
    Eigen::MatrixXd E_IJ = bindColumns(I.EncodedProfile, J.EncodedProfile);

    int columnIndex = RV.generate_uniform_binary();

    std::size_t N = E_IJ.rows();
    Eigen::VectorXd E = Eigen::VectorXd::Zero(N);
    for (std::size_t n = 0; n < N; n++)
    {
        double p = RV.generate_uniform_real();
        if (p < CrossoverProbability)
        {
            columnIndex = (columnIndex + 1) % 2;
        }

        E[n] = E_IJ.row(n)[columnIndex];
    }

    Individual K(E, ES);
    RestructingIndividual(K, ES);

    return K;
}

Eigen::VectorXd EvolutionaryAlgorithm::CreateMutationProbability(Individual & I, const ExperimentalSetup & ES)
{
    const Eigen::VectorXd & E = I.EncodedProfile;
    const double & referenceMarkerAverage = I.SampleParameters[0];
    const double & dispersion = I.SampleParameters[1];

    const Eigen::VectorXd & Coverage = ES.Coverage;
    const Eigen::VectorXd & MarkerImbalance = I.MarkerImbalanceParameters;
    const Eigen::VectorXd & NumberOfAlleles = ES.NumberOfAlleles;

    std::size_t N = ES.Coverage.size();
    boost::math::students_t devianceDistribution(MutationDegreesOfFreedom);

    Eigen::VectorXd mutation = MutationProbabilityLowerLimit * Eigen::VectorXd::Ones(E.size());
    if (std::abs(MutationDecay_t - MutationProbabilityLowerLimit) > DBL_EPSILON)
    {
        for (std::size_t m = 0; m < MarkerImbalance.size(); m++)
        {
            const Eigen::VectorXd & alleleIndex_m = I.ReducedAlleleIndex[m];
            const std::size_t A = alleleIndex_m.size();

            const Eigen::MatrixXd & expectedContributionProfile_m = I.ReducedExpectedContributionMatrix[m];
            const Eigen::VectorXd & EC = expectedContributionProfile_m * I.MixtureParameters;

            const Eigen::VectorXd & E_m = E.segment(2 * (ES.NumberOfContributors - ES.NumberOfKnownContributors) * m, 2 * (ES.NumberOfContributors - ES.NumberOfKnownContributors));
            for (std::size_t i = 0; i < E_m.size(); i++)
            {
                const std::size_t & n = ES.PartialSumAlleles[m] + E_m[i];
                std::size_t a = 0;
                while ((alleleIndex_m[a] != E_m[i]) & (a < A - 1))
                {
                    a++;
                }

                double mu_ma = referenceMarkerAverage * MarkerImbalance[m] * EC[a];
                if ((mu_ma == 0) & (Coverage[n] != 0))
                {
                    mu_ma += 2e-8;
                }

                double deviance_n = devianceResidualPoissonGammaDistribution(Coverage[n], mu_ma, mu_ma / dispersion);
                if (std::isnan(deviance_n))
                {
                    deviance_n = 0.0;
                    Rcpp::warning("Deviance returned 'nan'.");
                }
                else if (std::isinf(deviance_n))
                {
                    deviance_n = HUGE_VAL;
                    Rcpp::warning("Deviance returned 'inf'.");
                }

                mutation[2 * (ES.NumberOfContributors - ES.NumberOfKnownContributors) * m + i] =
                    MutationDecay_t - (MutationDecay_t - MutationProbabilityLowerLimit) * boost::math::pdf(devianceDistribution, deviance_n) /
                        boost::math::pdf(devianceDistribution, 0.0);
            }
        }
    }

    return mutation;
}

void updateMarkerIndividual(const Eigen::VectorXd & E, std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix, std::vector<Eigen::VectorXd> reducedAlleleIndex,
                            std::vector<Eigen::VectorXd> reducedNoiseIndex, const std::size_t & m, const ExperimentalSetup & ES)
{
    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    // Creating genotype matrix of marker m
    Eigen::MatrixXd decodedProfile_m = Eigen::MatrixXd::Zero(ES.NumberOfAlleles[m], ES.NumberOfContributors);
    for (std::size_t i = 0; i < ES.NumberOfAlleles[m]; i++)
    {
        for (std::size_t j = 0; j < ES.NumberOfKnownContributors; j++)
        {
            decodedProfile_m(i, j) = ES.KnownProfiles(ES.PartialSumAlleles[m] + i, j);
        }
    }

    for (std::size_t i = 0; i < numberOfUnknownContributors; i++)
    {
        for (std::size_t j = 0; j <= 1; j++)
        {
            std::size_t l = 2 * m * numberOfUnknownContributors + 2 * i + j;
            decodedProfile_m(E[l], i + ES.NumberOfKnownContributors) += 1;
        }
    }

    // Creating ECM of row n
    std::vector<Eigen::MatrixXd> potentialParents_m = ES.PotentialParents[m];
    Eigen::MatrixXd expectedContributionProfile_m = Eigen::MatrixXd::Zero(ES.NumberOfAlleles[m], ES.NumberOfContributors);
    for (std::size_t u = 0; u < ES.NumberOfContributors; u++)
    {
        Eigen::VectorXd decodedProfile_mu = decodedProfile_m.col(u);
        Eigen::VectorXd stutterContribution = Eigen::VectorXd::Zero(ES.NumberOfAlleles[m]);
        for (std::size_t a = 0; a < ES.NumberOfAlleles[m]; a++)
        {
            stutterContribution[a] = ParentStutterContribution(a, ES.LevelsOfStutterRecursion, 1, decodedProfile_mu, potentialParents_m, ES.NumberOfAlleles[m]);
        }

        expectedContributionProfile_m.col(u) = decodedProfile_mu + stutterContribution;
    }

    double noiseProfileSum_m = 0.0;
    double noiseProfileSize_m = expectedContributionProfile_m.rows();
    for (std::size_t i = 0; i < noiseProfileSize_m; i++)
    {
        const double & ecps = expectedContributionProfile_m.row(i).sum();
        if (!(ecps > 2e-16))
        {
            noiseProfileSum_m++;
        }
    }

    Eigen::MatrixXd reducedExpectedContributionMatrix_m = Eigen::MatrixXd::Zero(noiseProfileSize_m - noiseProfileSum_m, ES.NumberOfContributors);
    Eigen::VectorXd reducedAlleleIndex_m = Eigen::VectorXd::Zero(noiseProfileSize_m - noiseProfileSum_m);
    Eigen::VectorXd reducedNoiseIndex_m = Eigen::VectorXd::Zero(noiseProfileSum_m);

    std::size_t n = 0;
    std::size_t i = 0, j = 0;
    for (std::size_t a = 0; a < ES.NumberOfAlleles[m]; a++)
    {
        const Eigen::VectorXd & expectedContributionProfile_mn = expectedContributionProfile_m.row(n);

        if (expectedContributionProfile_mn.sum() > 0)
        {
            reducedExpectedContributionMatrix_m.row(i) = expectedContributionProfile_mn;
            reducedAlleleIndex_m[i] = a;
            i++;
        }
        else
        {
            reducedNoiseIndex_m[j] = a;
            j++;
        }

        n++;
    }

    reducedExpectedContributionMatrix[m] = reducedExpectedContributionMatrix_m;
    reducedAlleleIndex[m] = reducedAlleleIndex_m;
    reducedNoiseIndex[m] = reducedNoiseIndex_m;
}

void EvolutionaryAlgorithm::Mutation(Individual & I, const ExperimentalSetup & ES, RandomVariates & RV)
{
    std::size_t numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    Eigen::VectorXd E = I.EncodedProfile;
    Eigen::VectorXd encodedMutation = CreateMutationProbability(I, ES);

    // const std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix = I.ReducedExpectedContributionMatrix;
    // const std::vector<Eigen::VectorXd> reducedAlleleIndex = I.ReducedAlleleIndex;
    // const std::vector<Eigen::VectorXd> reducedNoiseIndex = I.ReducedNoiseIndex;
    for (std::size_t m = 0; m < ES.NumberOfMarkers; m++)
    {
        std::size_t j = 0;
        const std::size_t i = 2 * numberOfUnknownContributors * m;
        for (std::size_t k = 0; k < 2 * numberOfUnknownContributors; k++)
        {
            double mutate = RV.generate_uniform_real();
            if (mutate < encodedMutation[i + k])
            {
                int mutationShift = 0;
                if (ES.NumberOfAlleles[m] > 1)
                {
                    while(mutationShift == 0)
                    {
                        mutationShift += RV.generate_uniform_mutation[m]();
                    }
                }

                const double E_k = E[i + k];
                E[i + k] = static_cast<int>(E_k + mutationShift) % static_cast<int>(ES.NumberOfAlleles[m]);
                j++;
            }
        }

        // if (j > 0)
        // {
        //     updateMarkerIndividual(E, reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, m, ES);
        // }
    }

    Individual J(E, ES); // J(E, reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, ES);
    I = J;
}

Eigen::VectorXd expectedContributionMatrixRow(const Eigen::VectorXd & E, const ExperimentalSetup & ES,
                                              const int & m, const std::size_t & k, const std::size_t & n)
{

    const std::size_t & a = E[k];
    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    // Creating genotype matrix of marker m
    Eigen::MatrixXd decodedProfile_m = Eigen::MatrixXd::Zero(ES.NumberOfAlleles[m], ES.NumberOfContributors);
    for (std::size_t i = 0; i < ES.NumberOfAlleles[m]; i++)
    {
        for (std::size_t j = 0; j < ES.NumberOfKnownContributors; j++)
        {
            decodedProfile_m(i, j) = ES.KnownProfiles(ES.PartialSumAlleles[m] + i, j);
        }
    }

    for (std::size_t i = 0; i < numberOfUnknownContributors; i++)
    {
        for (std::size_t j = 0; j <= 1; j++)
        {
            std::size_t l = 2 * m * numberOfUnknownContributors + 2 * i + j;
            decodedProfile_m(E[l], i + ES.NumberOfKnownContributors) += 1;
        }
    }

    // Creating ECM of row n
    std::vector<Eigen::MatrixXd> potentialParents_m = ES.PotentialParents[m];
    Eigen::VectorXd ECM_n = Eigen::VectorXd::Zero(decodedProfile_m.cols());
    for (std::size_t j = 0; j < ECM_n.size(); j++)
    {
        const Eigen::VectorXd & decodedProfile_mj = decodedProfile_m.col(j);

        double potentialParentContribution = ParentStutterContribution(a, ES.LevelsOfStutterRecursion, 1, decodedProfile_mj, potentialParents_m, ES.NumberOfAlleles[m]);
        ECM_n[j] = decodedProfile_mj[a] + potentialParentContribution;
    }

    return ECM_n;
}

void EvolutionaryAlgorithm::HillClimbing(Individual & I, ExperimentalSetup & ES, RandomVariates & RV)
{
    std::size_t numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    for (std::size_t i = 0; i < HillClimbingIterations; i++)
    {
        int stepMarker = RV.generate_uniform_marker();
        int stepContributor = RV.generate_uniform_unknown_contributor();
        int stepBinary = RV.generate_uniform_binary();
        std::size_t k = 2 * stepMarker * numberOfUnknownContributors + 2 * stepContributor + stepBinary;

        // Create n'th row of the decoded and ecm matrix
        std::size_t n = ES.PartialSumAlleles[stepMarker] + I.EncodedProfile[k];
        Eigen::VectorXd expectedContributionMatrixRow_n_i = expectedContributionMatrixRow(I.EncodedProfile, ES, stepMarker, k, n);

        double I_mu_ma = I.SampleParameters[0] * ES.MarkerImbalances[n] * expectedContributionMatrixRow_n_i.transpose() * I.MixtureParameters;

        std::vector< Eigen::VectorXd > surroundings(ES.NumberOfAlleles[stepMarker] - 1);
        Eigen::VectorXd surroundingResiduals = Eigen::VectorXd::Zero(ES.NumberOfAlleles[stepMarker] - 1);
        for (std::size_t j = 1; j < ES.NumberOfAlleles[stepMarker]; j++)
        {
            Eigen::VectorXd I_j = I.EncodedProfile;

            I_j[k] = static_cast<int>(I_j[k] + j) % static_cast<int>(ES.NumberOfAlleles[stepMarker]);
            std::size_t m = ES.PartialSumAlleles[stepMarker] + I_j[k];

            Eigen::VectorXd expectedContributionMatrixRow_n_j = expectedContributionMatrixRow(I_j, ES, stepMarker, k, m);
            double J_mu_ma = I.SampleParameters[0] * ES.MarkerImbalances[m] * expectedContributionMatrixRow_n_j.transpose() * I.MixtureParameters;

            surroundings[j - 1] = I_j;
            surroundingResiduals[j - 1] = std::abs(ES.Coverage[n] - I_mu_ma + ES.Coverage[m] - J_mu_ma);
        }

        Eigen::MatrixXf::Index minIndex;
        double smallestValue = surroundingResiduals.minCoeff(&minIndex);

        // std::vector<Eigen::MatrixXd> reducedExpectedContributionMatrix = I.ReducedExpectedContributionMatrix;
        // std::vector<Eigen::VectorXd> reducedAlleleIndex = I.ReducedAlleleIndex;
        // std::vector<Eigen::VectorXd> reducedNoiseIndex = I.ReducedNoiseIndex;
        //
        // updateMarkerIndividual(surroundings[minIndex], reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, stepMarker, ES);

        Individual K(surroundings[minIndex], ES); // K(surroundings[minIndex], reducedExpectedContributionMatrix, reducedAlleleIndex, reducedNoiseIndex, ES);
        if (K.Fitness > I.Fitness)
        {
            I = K;
        }
    }
}

Population EvolutionaryAlgorithm::SelectionCrossoverMutation(const Population & P, ExperimentalSetup & ES, RandomVariates & RV)
{
    // Creating new child population
    std::vector<Individual> childPopulation(PopulationSize);
    for (std::size_t i = 0; i < PopulationSize; i++)
    {
        Individual parent = P.Individuals[i];

        // Parent partner selection
        std::size_t partnerIndex = ChoosePartner(P, i, RV);

        // Crossover
        Individual child = Crossover(parent, P.Individuals[partnerIndex], ES, RV);

        // Mutation
        Mutation(child, ES, RV);

        if (AllowParentSurvival)
        {
            // Hill-climbing
            if (HillClimbingIterations != 0)
            {
                HillClimbing(parent, ES, RV);
                RestructingIndividual(parent, ES);
            }

            if (parent.Fitness < child.Fitness)
            {
                // Restructuring
                RestructingIndividual(child, ES);
                childPopulation[i] = child;
            }
            else
            {
                childPopulation[i] = parent;
            }
        }
        else
        {
            // Hill-climbing
            if (HillClimbingIterations != 0)
            {
                HillClimbing(child, ES, RV);
            }

            // Restructuring
            RestructingIndividual(child, ES);
            childPopulation[i] = child;
        }

    }

    Population C(childPopulation);
    return C;
}

bool individualsEqual(Individual & I, Individual & J)
{
    Eigen::VectorXd K = I.EncodedProfile - J.EncodedProfile;
    return (K.sum() < 2e-16);
}

void EvolutionaryAlgorithm::Run(ExperimentalSetup & ES, RandomVariates & RV, const bool & trace)
{
    std::vector<Individual> TI(NumberOfFittestIndividuals);
    std::vector<double> TF(NumberOfFittestIndividuals, -HUGE_VAL);

    std::size_t n = 0;
    std::size_t terminationCounter = 0;
    bool terminate = false;
    while (!terminate)
    {
        // Updating current population
        MutationDecay_t = MutationDecay[n];

        Population C = SelectionCrossoverMutation(CurrentPopulation, ES, RV);
        CurrentPopulation = C;

        // Updating the list of fittest individuals
        std::vector<int> sortedFitness = sortedIndex(C.Fitness);

        std::size_t m = 0;
        bool updateFittest = true;
        while (updateFittest)
        {
            Individual I_m = C.Individuals[sortedFitness[m]];

            double F_m = I_m.Fitness;
            if (F_m > TF[0])
            {
                auto it_front = std::lower_bound(TF.cbegin(), TF.cend(), F_m);
                auto it_end = std::upper_bound(TF.cbegin(), TF.cend(), F_m);
                std::size_t i = it_front - TF.cbegin();
                std::size_t j = it_end - TF.cbegin();

                bool isDuplicate = false;
                if (F_m == TF[j - 1]) {
                    for (std::size_t k = i - 1; k < j; k++)
                    {
                        Eigen::VectorXd K = I_m.EncodedProfile - TI[k].EncodedProfile;
                        double sumAbs = 0.0;
                        for (std::size_t h = 0; h < K.size(); h++) {
                            sumAbs += std::abs(K[h]);
                        }

                        isDuplicate = (sumAbs < 2e-16);
                    }
                }

                if (!isDuplicate)
                {
                    for (std::size_t k = 0; k < j - 1; k++)
                    {
                        TI[k] = TI[k + 1];
                        TF[k] = TF[k + 1];
                    }

                    TI[j - 1] = I_m;
                    TF[j - 1] = F_m;
                }
            }

            m++;
            updateFittest = (m < NumberOfFittestIndividuals) & (F_m > TF[0]);
        }

        // Updates the termination counter
        if (std::abs(C.Fitness[sortedFitness[0]] - C.Fitness.mean()) / std::abs(C.Fitness[sortedFitness[0]]) < ES.Tolerance[0])
        {
            terminationCounter++;
        }
        else
        {
            terminationCounter = 0;
        }

        n++;
        terminate = (terminationCounter >= NumberOfIterationsEqualMinMax) || (n >= NumberOfIterations);

        if (trace)
        {
            Rcpp::Rcout << "\tCurrent iteration: " << n << "\n"
                        << "\t\tPopulation Fitness: " << "\n"
                        << "\t\t  Highest: " << C.Fitness[sortedFitness[0]] << "\n"
                        << "\t\t  Average: " << C.Fitness.mean() << "\n"
                        << "\t\t  Lowest: " << C.Fitness[sortedFitness[sortedFitness.size() - 1]] << "\n"
                        << "\t\tTermination counter: " << terminationCounter << " / " << NumberOfIterationsEqualMinMax << "\n";
        }
    }

    Population FittestMembers(TI);
    FittestMembersOfEntireRun = FittestMembers;
}


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 *                                                                            *
 * This program is free software; you can redistribute it and/or modify       *
 * it under the terms of the Lesser GNU General Public License as published by*
 * the Free Software Foundation; either version 3 of the License, or          *
 * (at your option) any later version.                                        *
 *                                                                            *
 * This program 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              *
 * Lesser GNU General Public License for more details.                        *
 *                                                                            *
 * You should have received a copy of the Lesser GNU General Public License   *
 * along with this program. If not, see <http://www.gnu.org/licenses/>.       *
 ******************************************************************************/

#include "aslam/calibration/error-terms/ErrorTermAngularVelocity.h"

#include <Eigen/Dense>

using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

  ErrorTermAngularVelocity::ErrorTermAngularVelocity(const aslam::backend::EuclideanExpression& r_w_mr,
                                                     const aslam::backend::EuclideanExpression& r_w_mr_m,
                                                     const Covariance& sigma2_ang_velZ) :
        _r_w_mr(r_w_mr),
        _r_w_mr_m(r_w_mr_m),
        _sigma2_ang_velZ(sigma2_ang_velZ) {
      setInvR(_sigma2_ang_velZ.inverse());
      DesignVariable::set_t dv;
      _r_w_mr.getDesignVariables(dv);
      _r_w_mr_m.getDesignVariables(dv);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

  ErrorTermAngularVelocity::ErrorTermAngularVelocity(const ErrorTermAngularVelocity& other) :
        ErrorTermFs<1>(other),
        _r_w_mr(other. _r_w_mr),
        _r_w_mr_m(other. _r_w_mr_m),
        _sigma2_ang_velZ(other._sigma2_ang_velZ){
    }

  ErrorTermAngularVelocity& ErrorTermAngularVelocity::operator =
        (const ErrorTermAngularVelocity& other) {
      if (this != &other) {
        ErrorTermFs<1>::operator=(other);
        _r_w_mr = other._r_w_mr;
        _r_w_mr_m = other._r_w_mr_m;
       _sigma2_ang_velZ = other._sigma2_ang_velZ;
      }
      return *this;
    }

    ErrorTermAngularVelocity::~ErrorTermAngularVelocity() {
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermAngularVelocity::evaluateErrorImplementation() {
      error_t error;
      const double wz = _r_w_mr.toEuclidean()(2);
      const double wzm = _r_w_mr_m.toEuclidean()(2);

      error(0) = wz - wzm;

      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermAngularVelocity::evaluateJacobiansImplementation(JacobianContainer&
        jacobians) {
      Eigen::Matrix<double, 1, 3> Jw = Eigen::Matrix<double, 1, 3>::Zero();
      Eigen::Matrix<double, 1, 3> Jwm = Eigen::Matrix<double, 1, 3>::Zero();

      Jw(2) = 1.0;
      Jwm(2) = 1.0;

      _r_w_mr.evaluateJacobians(jacobians, Jw);
      _r_w_mr_m.evaluateJacobians(jacobians, -Jwm);
    }

  }
}


/*

Copyright (c) 2005-2020, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/

#ifndef _ABSTRACTLINEARELLIPTICPDE_HPP_
#define _ABSTRACTLINEARELLIPTICPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractLinearPde.hpp"
#include "UblasCustomFunctions.hpp"
#include "ChastePoint.hpp"
#include "Node.hpp"
#include "Element.hpp"
#include <petscvec.h>

/**
 * AbstractLinearEllipticPde class.
 *
 * A general PDE of the form:
 * 0 =   Grad.(DiffusionTerm(x)*Grad(u))
 *     + ComputeConstantInUSourceTerm(x)
 *     + ComputeLinearInUCoeffInSourceTerm(x, u)
 *
 * Parabolic PDEs are be derived from this (AbstractLinearParabolicPde)
 */
template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
class AbstractLinearEllipticPde : public AbstractLinearPde<ELEMENT_DIM, SPACE_DIM>
{
private:

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Serialize the PDE object.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractLinearPde<ELEMENT_DIM, SPACE_DIM> >(*this);
    }

public:

    /**
     * Constructor.
     */
    AbstractLinearEllipticPde()
        : AbstractLinearPde<ELEMENT_DIM, SPACE_DIM>()
    {}

    /**
     * Destructor.
     */
    virtual ~AbstractLinearEllipticPde()
    {}

    /**
     * @return computed constant in u part of the source term, i.e g(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given point.
     *
     * @param rX The point in space
     * @param pElement The element
     */
    virtual double ComputeConstantInUSourceTerm(const ChastePoint<SPACE_DIM>& rX,
                                                Element<ELEMENT_DIM,SPACE_DIM>* pElement)=0;

    /**
     * @return computed coefficient of u in the linear part of the source term, i.e f(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given point in space.
     *
     * @param rX The point in space
     * @param pElement
     */
    virtual double ComputeLinearInUCoeffInSourceTerm(const ChastePoint<SPACE_DIM>& rX,
                                                     Element<ELEMENT_DIM,SPACE_DIM>* pElement)=0;

    /**
     * @return computed diffusion term at a given point. The diffusion tensor should be symmetric and positive definite
     *
     * @param rX The point in space at which the diffusion term is computed.
     * @return A matrix.
     */
    virtual c_matrix<double, SPACE_DIM, SPACE_DIM> ComputeDiffusionTerm(const ChastePoint<SPACE_DIM>& rX)=0;

    /**
     * @return computed constant in u part of the source term, i.e g(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given node.
     *
     * @param rNode the node
     */
    virtual double ComputeConstantInUSourceTermAtNode(const Node<SPACE_DIM>& rNode);

    /**
     * @return computed coefficient of u in the linear part of the source term, i.e f(x) in
     * Div(D Grad u)  +  f(x)u + g(x) = 0, at a given node.
     *
     * @param rNode the node
     */
    virtual double ComputeLinearInUCoeffInSourceTermAtNode(const Node<SPACE_DIM>& rNode);
};

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double AbstractLinearEllipticPde<ELEMENT_DIM, SPACE_DIM>::ComputeConstantInUSourceTermAtNode(const Node<SPACE_DIM>& rNode)
{
    return ComputeConstantInUSourceTerm(rNode.GetPoint(), nullptr);
}

template <unsigned ELEMENT_DIM, unsigned SPACE_DIM>
double AbstractLinearEllipticPde<ELEMENT_DIM, SPACE_DIM>::ComputeLinearInUCoeffInSourceTermAtNode(const Node<SPACE_DIM>& rNode)
{
    return ComputeLinearInUCoeffInSourceTerm(rNode.GetPoint(), nullptr);
}

#endif //_ABSTRACTLINEARELLIPTICPDE_HPP_


/**
 *
 * RenderPipeline
 *
 * Copyright (c) 2014-2016 tobspr <tobias.springer1@gmail.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 */

// Only include the pssm helper if actually required
#ifdef RP_REQ_PSSM_HELPER

#include "pssm_helper.h"

#include <Eigen/Dense>

/**
 * @brief Internal method to generate a set of equations
 * @details This generates a set of equations into a given equation system.
 *   Based on a given start point and end point, it inserts an equation to the
 *   system which will, when solved, solve the following equation:
 *   vec * <SOLVED-TRANSFORM> = expected
 *
 * @param eq_system The equation system
 * @param eq_results The target result vector of the equation system
 * @param vec The actual vector to be transformed
 * @param expected The expected output of the transformation
 * @param offset The index of the equation
 */
void generate_equations(Eigen::MatrixXf &eq_system, Eigen::VectorXf &eq_results, const LVector4f &vec, LVector4f expected, size_t offset) {
    size_t write_offset = offset * 4;
    for (size_t row = 0; row < 4; ++row) {
        float expected_coeff = expected.get_cell(row);
        size_t col_offset = row * 4;
        for (size_t col = 0; col < 4; ++col) {
            eq_system(write_offset, col_offset + col) = vec.get_cell(col);
        }
        eq_results(write_offset++) = expected_coeff;
    }
}
/**
 * @brief Finds a projection mat arround the given set of points.
 * @details This methods finds a projection matrix which projects the given set
 *   of frustum points to a unit cube, which can be used as a camera matrix.
 *   The eight points should determine the frustum uniquely.
 *
 *  @param near_ul The Upper-Left point of the frustum on the near plane
 *  @param near_ur The Upper-Right point of the frustum on the near plane
 *  @param near_ll The Lower-Left point of the frustum on the near plane
 *  @param near_lr The Lower-Right point of the frustum on the near plane
 *  @param far_ul The Upper-Left point of the frustum on the far plane
 *  @param far_ur The Upper-Right point of the frustum on the far plane
 *  @param far_ll The Lower-Left point of the frustum on the far plane
 *  @param far_lr The Lower-Right point of the frustum on the far plane
 *
 */
LMatrix4f PSSMHelper::find_projection_mat(
            const LVector4f &near_ul,
            const LVector4f &near_ur,
            const LVector4f &near_ll,
            const LVector4f &near_lr,

            const LVector4f &far_ul,
            const LVector4f &far_ur,
            const LVector4f &far_ll,
            const LVector4f &far_lr) {

    // We have 8*4 = 32 equations, which require 16 coefficients each
    Eigen::MatrixXf equation_system(32, 16);
    Eigen::VectorXf equation_results(32);
    equation_system.fill(0);

    // Generate the equations
    size_t offset = 0;
    generate_equations(equation_system, equation_results, near_ul, LVector4f(-1,  1, 0, 1), offset++);
    generate_equations(equation_system, equation_results, near_ur, LVector4f( 1,  1, 0, 1), offset++);
    generate_equations(equation_system, equation_results, near_ll, LVector4f(-1, -1, 0, 1), offset++);
    generate_equations(equation_system, equation_results, near_lr, LVector4f( 1, -1, 0, 1), offset++);

    generate_equations(equation_system, equation_results, far_ul,  LVector4f(-1,  1, 1, 1), offset++);
    generate_equations(equation_system, equation_results, far_ur,  LVector4f( 1,  1, 1, 1), offset++);
    generate_equations(equation_system, equation_results, far_ll,  LVector4f(-1, -1, 1, 1), offset++);
    generate_equations(equation_system, equation_results, far_lr,  LVector4f( 1, -1, 1, 1), offset++);

    // Solve the equation system
    Eigen::VectorXf solved_system = equation_system.colPivHouseholderQr().solve(equation_results);

    // Construct result matrix and return it. We also need to transpose the matrix.
    LMatrix4f result(
            solved_system(0), solved_system(4), solved_system(8),  solved_system(12),
            solved_system(1), solved_system(5), solved_system(9),  solved_system(13),
            solved_system(2), solved_system(6), solved_system(10), solved_system(14),
            solved_system(3), solved_system(7), solved_system(11), solved_system(15)
        );
    return result;
}

#endif // RP_REQ_PSSM_HELPER