/**
 * @file constrained_ik.cpp
 * @brief Constrained Inverse Kinematic Solver
 *
 * @author dsolomon
 * @date Sep 15, 2013
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2013, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <constrained_ik/constrained_ik.h>
#include "constrained_ik/constraint_group.h"
#include <boost/make_shared.hpp>
#include <constrained_ik/constraint_results.h>
#include <ros/ros.h>

const std::vector<std::string> SUPPORTED_COLLISION_DETECTORS = {"IndustrialFCL", "CollisionDetectionOpenVDB"}; /**< Supported collision detector */

namespace constrained_ik
{
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Affine3d;

Constrained_IK::Constrained_IK():nh_("~")
{
  initialized_ = false;

  loadDefaultSolverConfiguration();
}

void Constrained_IK::addConstraintsFromParamServer(const std::string &parameter_name)
{
  XmlRpc::XmlRpcValue constraints_xml;
  boost::shared_ptr<pluginlib::ClassLoader<constrained_ik::Constraint> > constraint_loader;

  constraint_loader.reset(new pluginlib::ClassLoader<constrained_ik::Constraint>("constrained_ik", "constrained_ik::Constraint"));

  if (!nh_.getParam(parameter_name, constraints_xml))
  {
    ROS_ERROR("Unable to find ros parameter: %s", parameter_name.c_str());
    ROS_BREAK();
    return;
  }

  if(constraints_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("ROS parameter %s must be an array", parameter_name.c_str());
    ROS_BREAK();
    return;
  }

  for (int i=0; i<constraints_xml.size(); ++i)
  {
    XmlRpc::XmlRpcValue constraint_xml = constraints_xml[i];

    if (constraint_xml.hasMember("class") &&
              constraint_xml["class"].getType() == XmlRpc::XmlRpcValue::TypeString &&
              constraint_xml.hasMember("primary") &&
              constraint_xml["primary"].getType() == XmlRpc::XmlRpcValue::TypeBoolean)
    {
      std::string class_name = constraint_xml["class"];
      bool is_primary = constraint_xml["primary"];

      Constraint *constraint;
      try
      {
        constraint = constraint_loader->createUnmanagedInstance(class_name);

        constraint->loadParameters(constraint_xml);
        if (is_primary)
          addConstraint(constraint, constraint_types::Primary);
        else
          addConstraint(constraint, constraint_types::Auxiliary);

      }
      catch (pluginlib::PluginlibException& ex)
      {
        ROS_ERROR("Couldn't load constraint named %s.\n Error: %s", class_name.c_str(), ex.what());
        ROS_BREAK();
      }
    }
    else
    {
      ROS_ERROR("Constraint must have class(string) and primary(boolean) members");
    }
  }
}

void Constrained_IK::loadDefaultSolverConfiguration()
{
  ConstrainedIKConfiguration config;
  config.debug_mode = false;
  config.allow_joint_convergence = false;
  config.allow_primary_normalization = true;
  config.allow_auxiliary_nomalization = true;
  config.limit_primary_motion = false;
  config.limit_auxiliary_motion = false;
  config.limit_auxiliary_interations = false;
  config.solver_max_iterations = 500;
  config.solver_min_iterations = 0;
  config.auxiliary_max_iterations = 5;
  config.primary_max_motion = 2.0;
  config.auxiliary_max_motion = 0.2;
  config.primary_norm = 1.0;
  config.auxiliary_norm = 0.2;
  config.primary_gain = 1.0;
  config.auxiliary_gain = 1.0;
  config.joint_convergence_tol = 0.0001;

  setSolverConfiguration(config);
}

void Constrained_IK::setSolverConfiguration(const ConstrainedIKConfiguration &config)
{
  config_ = config;
  validateConstrainedIKConfiguration<ConstrainedIKConfiguration>(config_);
}

constrained_ik::ConstraintResults Constrained_IK::evalConstraint(constraint_types::ConstraintTypes constraint_type, const constrained_ik::SolverState &state) const
{
  switch(constraint_type)
  {
    case constraint_types::Primary:
      return primary_constraints_.evalConstraint(state);
    case constraint_types::Auxiliary:
      return auxiliary_constraints_.evalConstraint(state);
  }
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjection(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv = calcDampedPseudoinverse(J);
  MatrixXd JplusJ = J_pinv * J;
  int mn = JplusJ.rows();
  MatrixXd P = MatrixXd::Identity(mn,mn)-JplusJ;
  return (P);
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjectionTheRightWay(const Eigen::MatrixXd &A) const
{
  Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullV);
  MatrixXd V(svd.matrixV());

  // determine rank using same algorithym as latest Eigen
  // TODO replace next 10 lines of code with rnk = svd.rank(); once eigen3 is updated
  int rnk = 0;
  if(svd.singularValues().size()==0)
  {
    rnk = 0;
  }
  else
  {
    double threshold = std::min(A.rows(),A.cols())*Eigen::NumTraits<double>::epsilon();
    double premultiplied_threshold = svd.singularValues().coeff(0) * threshold;
    rnk = svd.nonzeroSingularValues()-1;
    while(rnk>=0 && svd.singularValues().coeff(rnk) < premultiplied_threshold) --rnk;
    rnk++;
  }

  // zero singular vectors in the range of A
  for(int i=0; i<rnk; ++i)
  {
    for(int j=0; j<A.cols(); j++) V(j,i) = 0;
  }

  MatrixXd P = V *  V.transpose();
  return(P);
}

Eigen::MatrixXd Constrained_IK::calcDampedPseudoinverse(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv;

  if (basic_kin::BasicKin::dampedPInv(J,J_pinv))
  {
    return J_pinv;
  }
  else
  {
    ROS_ERROR_STREAM("Not able to calculate damped pseudoinverse!");
    throw std::runtime_error("Not able to calculate damped pseudoinverse!  IK solution may be invalid.");
  }
}

bool Constrained_IK::calcInvKin(const Eigen::Affine3d &goal,
                                const Eigen::VectorXd &joint_seed,
                                Eigen::VectorXd &joint_angles) const
{
  return calcInvKin(goal,joint_seed, planning_scene::PlanningSceneConstPtr(), joint_angles);
}

bool Constrained_IK::calcInvKin(const Eigen::Affine3d &goal,
                                const Eigen::VectorXd &joint_seed,
                                const planning_scene::PlanningSceneConstPtr planning_scene,
                                Eigen::VectorXd &joint_angles) const
{
  double dJoint_norm;
  SolverStatus status;

    // initialize state
  joint_angles = joint_seed;  // initialize result to seed value
  constrained_ik::SolverState state = getState(goal, joint_seed); // create state vars for this IK solve
  state.condition = checkInitialized();
  state.planning_scene = planning_scene;
  state.group_name = kin_.getJointModelGroup()->getName();

  //TODO: Does this still belong here?
  if(planning_scene)
  {
    state.robot_state = robot_state::RobotStatePtr(new moveit::core::RobotState(planning_scene->getCurrentState()));

    //Check and make sure the correct collision detector is loaded.
    auto pos = std::find(SUPPORTED_COLLISION_DETECTORS.begin(),SUPPORTED_COLLISION_DETECTORS.end(),
                         planning_scene->getActiveCollisionDetectorName());
    if (pos == SUPPORTED_COLLISION_DETECTORS.end())
    {
      std::stringstream error_message;
      error_message<<" Constrained IK requires the use of collision detectors: ";
      for(auto& d : SUPPORTED_COLLISION_DETECTORS)
      {
        error_message<<"'"<< d<<"' ";
      }
      error_message<<".\nSet or add the 'collision_detector' parameter to an allowed collision detector in the move_group.launch file"<<std::endl;
      throw std::runtime_error(error_message.str());
    }

    state.collision_robot = boost::dynamic_pointer_cast<const collision_detection::CollisionRobotIndustrial>(planning_scene->getCollisionRobot());
    state.collision_world = boost::dynamic_pointer_cast<const collision_detection::CollisionWorldIndustrial>(planning_scene->getCollisionWorld());
  }

  if (state.condition == initialization_state::NothingInitialized || state.condition == initialization_state::AuxiliaryOnly)
    throw std::runtime_error("Must call init() before using Constrained_IK and have a primary constraint.");

  //Cache the joint angles to return if max iteration is reached.
  Eigen::VectorXd cached_joint_angles = joint_seed;

  // iterate until solution converges (or aborted)
  while (true)
  {
    // re-update internal state variables
    updateState(state, joint_angles);

    // calculate a Jacobian (relating joint-space updates/deltas to cartesian-space errors/deltas)
    // and the associated cartesian-space error/delta vector
    // Primary Constraints
    constrained_ik::ConstraintResults primary = evalConstraint(constraint_types::Primary, state);
    // TODO since we already have J_p = USV, use that to get null-projection too.
    // otherwise, we are repeating the expensive calculation of the SVD

    VectorXd dJoint_p;
    dJoint_p.setZero(joint_seed.size());
    if (!primary.isEmpty()) // This is required because not all constraints always return data.
    {
      MatrixXd Ji_p = calcDampedPseudoinverse(primary.jacobian);
      dJoint_p = config_.primary_gain*(Ji_p*primary.error);
      dJoint_norm = dJoint_p.norm();
      if(config_.allow_primary_normalization && dJoint_norm > config_.primary_norm)// limit maximum update radian/meter
      {
        dJoint_p = config_.primary_norm * (dJoint_p/dJoint_norm);
        dJoint_norm = dJoint_p.norm();
      }
      state.primary_sum += dJoint_norm;
    }

    // Auxiliary Constraints
    VectorXd dJoint_a;
    constrained_ik::ConstraintResults auxiliary;
    dJoint_a.setZero(dJoint_p.size());
    if (state.condition == initialization_state::PrimaryAndAuxiliary)
    {
      if (!((config_.limit_auxiliary_motion && state.auxiliary_sum >= config_.auxiliary_max_motion) ||
          (config_.limit_auxiliary_interations && state.iter > config_.auxiliary_max_iterations)))
      {
        auxiliary = evalConstraint(constraint_types::Auxiliary, state);
        if (!auxiliary.isEmpty()) // This is required because not all constraints always return data.
        {
          MatrixXd N_p = calcNullspaceProjectionTheRightWay(primary.jacobian);
          MatrixXd Jnull_a = calcDampedPseudoinverse(auxiliary.jacobian*N_p);
          dJoint_a = config_.auxiliary_gain*Jnull_a*(auxiliary.error-auxiliary.jacobian*dJoint_p);
          dJoint_norm = dJoint_a.norm();
          if(config_.allow_auxiliary_nomalization && dJoint_norm > config_.auxiliary_norm)// limit maximum update radian/meter
          {
            dJoint_a = config_.auxiliary_norm * (dJoint_a/dJoint_norm);
            dJoint_norm = dJoint_a.norm();
          }
          state.auxiliary_sum += dJoint_norm;
        }
      }
      else
      {
        state.auxiliary_at_limit = true;
      }
    }

    status = checkStatus(state, primary, auxiliary);

    if (status == Converged)
    {
      ROS_DEBUG_STREAM("Found IK solution in " << state.iter << " iterations: " << joint_angles.transpose());
      return true;
    }
    else if (status == NotConverged)
    {
      // update joint solution by the calculated update (or a partial fraction)
      joint_angles += (dJoint_p + dJoint_a);
      clipToJointLimits(joint_angles);
    }
    else if (status == Failed)
    {
      joint_angles = cached_joint_angles;
      return false;
    }
  }
}

SolverStatus Constrained_IK::checkStatus(const constrained_ik::SolverState &state, const constrained_ik::ConstraintResults &primary, const constrained_ik::ConstraintResults &auxiliary) const
{
  // Check the status of convergence
  if(state.condition == initialization_state::PrimaryAndAuxiliary)
  {
    bool status = (primary.status && auxiliary.status);

    if (state.iter > config_.solver_min_iterations)
    {
      if (!status && primary.status && state.auxiliary_at_limit)
      {
        ROS_DEBUG("Auxiliary motion or iteration limit reached!");
        return Converged;
      }
      else if(status)
      {
        return Converged;
      }
    }
  }

  if(state.condition == initialization_state::PrimaryOnly)
  {
    if (primary.status && state.iter > config_.solver_min_iterations)
    {
      return Converged;
    }
  }

  // check for joint convergence
  //   - this is an error: joints stabilize, but goal pose not reached
  if (config_.allow_joint_convergence)
  {
    if (state.joints_delta.cwiseAbs().maxCoeff() < config_.joint_convergence_tol)
    {
      if (state.iter > config_.solver_min_iterations)
      {
        ROS_DEBUG_STREAM("Joint convergence reached " << state.iter << " / " << config_.solver_max_iterations << " iterations before convergence.");
        return Converged;
      }
    }
  }

  if (state.iter > config_.solver_max_iterations || (config_.limit_primary_motion && state.primary_sum >= config_.primary_max_motion))
  {
    if (!primary.status)
    {
      if (config_.limit_primary_motion && state.primary_sum >= config_.primary_max_motion)
      {
        ROS_WARN_STREAM("Primary reached max allowed motion, no solution returned.");
      }
      else
      {
        ROS_WARN_STREAM("Solver reached max allowed iteration, no solution returned.");
      }
      return Failed;
    }
    else if (state.condition == initialization_state::PrimaryAndAuxiliary)
    {
      ROS_WARN_STREAM("Maximum iterations reached but primary converged so returning solution.");
      return Converged;
    }
  }

  return NotConverged;
}

void Constrained_IK::clearConstraintList()
{
  primary_constraints_.clear();
  auxiliary_constraints_.clear();
}

void Constrained_IK::clipToJointLimits(Eigen::VectorXd &joints) const
{
  const MatrixXd limits = kin_.getLimits();
  const VectorXd orig_joints(joints);

  if (joints.size() != limits.rows())
    throw std::invalid_argument("clipToJointLimits: Unexpected number of joints");

  for (size_t i=0; i<limits.rows(); ++i)
  {
    joints[i] = std::max(limits(i,0), std::min(limits(i,1), joints[i]));
  }
  if (config_.debug_mode && !joints.isApprox(orig_joints))
      ROS_WARN("Joints have been clipped");
}

void Constrained_IK::init(const basic_kin::BasicKin &kin)
{
  if (!kin.checkInitialized())
    throw std::invalid_argument("Input argument 'BasicKin' must be initialized");

  kin_ = kin;
  initialized_ = true;
  primary_constraints_.init(this);
  auxiliary_constraints_.init(this);

}

double Constrained_IK::rangedAngle(double angle)
{
    angle = copysign(fmod(fabs(angle),2.0*M_PI), angle);
    if (angle < -M_PI) return angle+2.*M_PI;
    if (angle > M_PI)  return angle-2.*M_PI;
    return angle;
}

constrained_ik::SolverState Constrained_IK::getState(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed) const
{
  if (!kin_.checkJoints(joint_seed))
    throw std::invalid_argument("Seed doesn't match kinematic model");

  if (!goal.matrix().block(0,0,3,3).isUnitary(1e-6))
        throw std::invalid_argument("Goal pose not proper affine");

  return constrained_ik::SolverState(goal, joint_seed);
}

void Constrained_IK::updateState(constrained_ik::SolverState &state, const Eigen::VectorXd &joints) const
{
  // update maximum iterations
  state.iter++;

  // update joint convergence
  state.joints_delta = joints - state.joints;
  state.joints = joints;
  kin_.calcFwdKin(joints, state.pose_estimate);

  if(state.planning_scene && state.robot_state)
  {
    state.robot_state->setJointGroupPositions(kin_.getJointModelGroup()->getName(), joints);
    state.robot_state->update();
  }

  if (config_.debug_mode)
    state.iteration_path.push_back(joints);

}

} // namespace constrained_ik


#ifndef BOOST_GEOMETRY_PROJECTIONS_KROVAK_HPP
#define BOOST_GEOMETRY_PROJECTIONS_KROVAK_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

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

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017, 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: 4.9.1

// Original copyright notice:

// Purpose:  Implementation of the krovak (Krovak) projection.
// Definition: http://www.ihsenergy.com/epsg/guid7.html#1.4.3
// Author:   Thomas Flemming, tf@ttqv.com
// 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.

#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>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct krovak {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace krovak
    {
            template <typename T>
            struct par_krovak
            {
                T    C_x;
            };

            /**
               NOTES: According to EPSG the full Krovak projection method should have
                      the following parameters.  Within PROJ.4 the azimuth, and pseudo
                      standard parallel are hardcoded in the algorithm and can't be
                      altered from outside.  The others all have defaults to match the
                      common usage with Krovak projection.

              lat_0 = latitude of centre of the projection

              lon_0 = longitude of centre of the projection

              ** = azimuth (true) of the centre line passing through the centre of the projection

              ** = latitude of pseudo standard parallel

              k  = scale factor on the pseudo standard parallel

              x_0 = False Easting of the centre of the projection at the apex of the cone

              y_0 = False Northing of the centre of the projection at the apex of the cone

             **/

            // template class, using CRTP to implement forward/inverse
            template <typename CalculationType, typename Parameters>
            struct base_krovak_ellipsoid : public base_t_fi<base_krovak_ellipsoid<CalculationType, Parameters>,
                     CalculationType, Parameters>
            {

                typedef CalculationType geographic_type;
                typedef CalculationType cartesian_type;

                par_krovak<CalculationType> m_proj_parm;

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

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                /* calculate xy from lat/lon */

                /* Constants, identical to inverse transform function */
                    CalculationType s45, s90, e2, e, alfa, uq, u0, g, k, k1, n0, ro0, ad, a, s0, n;
                    CalculationType gfi, u, fi0, deltav, s, d, eps, ro;

                    s45 = 0.785398163397448;    /* 45 DEG */
                    s90 = 2 * s45;
                    fi0 = this->m_par.phi0;    /* Latitude of projection centre 49 DEG 30' */

                   /* Ellipsoid is used as Parameter in for.c and inv.c, therefore a must
                      be set to 1 here.
                      Ellipsoid Bessel 1841 a = 6377397.155m 1/f = 299.1528128,
                      e2=0.006674372230614;
                   */
                    a =  1; /* 6377397.155; */
                    /* e2 = this->m_par.es;*/       /* 0.006674372230614; */
                    e2 = 0.006674372230614;
                    e = sqrt(e2);

                    alfa = sqrt(1. + (e2 * pow(cos(fi0), 4)) / (1. - e2));

                    uq = 1.04216856380474;      /* DU(2, 59, 42, 42.69689) */
                    u0 = asin(sin(fi0) / alfa);
                    g = pow(   (1. + e * sin(fi0)) / (1. - e * sin(fi0)) , alfa * e / 2.  );

                    k = tan( u0 / 2. + s45) / pow  (tan(fi0 / 2. + s45) , alfa) * g;

                    k1 = this->m_par.k0;
                    n0 = a * sqrt(1. - e2) / (1. - e2 * pow(sin(fi0), 2));
                    s0 = 1.37008346281555;       /* Latitude of pseudo standard parallel 78 DEG 30'00" N */
                    n = sin(s0);
                    ro0 = k1 * n0 / tan(s0);
                    ad = s90 - uq;

                /* Transformation */

                    gfi =pow ( ((1. + e * sin(lp_lat)) /
                               (1. - e * sin(lp_lat))) , (alfa * e / 2.));

                    u= 2. * (atan(k * pow( tan(lp_lat / 2. + s45), alfa) / gfi)-s45);

                    deltav = - lp_lon * alfa;

                    s = asin(cos(ad) * sin(u) + sin(ad) * cos(u) * cos(deltav));
                    d = asin(cos(u) * sin(deltav) / cos(s));
                    eps = n * d;
                    ro = ro0 * pow(tan(s0 / 2. + s45) , n) / pow(tan(s / 2. + s45) , n)   ;

                   /* x and y are reverted! */
                    xy_y = ro * cos(eps) / a;
                    xy_x = ro * sin(eps) / a;

                        if( !pj_param(this->m_par.params, "tczech").i )
                      {
                        xy_y *= -1.0;
                        xy_x *= -1.0;
                      }
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    /* calculate lat/lon from xy */

                /* Constants, identisch wie in der Umkehrfunktion */
                    CalculationType s45, s90, fi0, e2, e, alfa, uq, u0, g, k, k1, n0, ro0, ad, a, s0, n;
                    CalculationType u, deltav, s, d, eps, ro, fi1, xy0;
                    int ok;

                    s45 = 0.785398163397448;    /* 45 DEG */
                    s90 = 2 * s45;
                    fi0 = this->m_par.phi0;    /* Latitude of projection centre 49 DEG 30' */

                   /* Ellipsoid is used as Parameter in for.c and inv.c, therefore a must
                      be set to 1 here.
                      Ellipsoid Bessel 1841 a = 6377397.155m 1/f = 299.1528128,
                      e2=0.006674372230614;
                   */
                    a = 1; /* 6377397.155; */
                    /* e2 = this->m_par.es; */      /* 0.006674372230614; */
                    e2 = 0.006674372230614;
                    e = sqrt(e2);

                    alfa = sqrt(1. + (e2 * pow(cos(fi0), 4)) / (1. - e2));
                    uq = 1.04216856380474;      /* DU(2, 59, 42, 42.69689) */
                    u0 = asin(sin(fi0) / alfa);
                    g = pow(   (1. + e * sin(fi0)) / (1. - e * sin(fi0)) , alfa * e / 2.  );

                    k = tan( u0 / 2. + s45) / pow  (tan(fi0 / 2. + s45) , alfa) * g;

                    k1 = this->m_par.k0;
                    n0 = a * sqrt(1. - e2) / (1. - e2 * pow(sin(fi0), 2));
                    s0 = 1.37008346281555;       /* Latitude of pseudo standard parallel 78 DEG 30'00" N */
                    n = sin(s0);
                    ro0 = k1 * n0 / tan(s0);
                    ad = s90 - uq;

                /* Transformation */
                   /* revert y, x*/
                    xy0=xy_x;
                    xy_x=xy_y;
                    xy_y=xy0;

                        if( !pj_param(this->m_par.params, "tczech").i )
                      {
                        xy_x *= -1.0;
                        xy_y *= -1.0;
                      }

                    ro = sqrt(xy_x * xy_x + xy_y * xy_y);
                    eps = atan2(xy_y, xy_x);
                    d = eps / sin(s0);
                    s = 2. * (atan(  pow(ro0 / ro, 1. / n) * tan(s0 / 2. + s45)) - s45);

                    u = asin(cos(ad) * sin(s) - sin(ad) * cos(s) * cos(d));
                    deltav = asin(cos(s) * sin(d) / cos(u));

                    lp_lon = this->m_par.lam0 - deltav / alfa;

                /* ITERATION FOR lp_lat */
                   fi1 = u;

                   ok = 0;
                   do
                   {
                       lp_lat = 2. * ( atan( pow( k, -1. / alfa)  *
                                            pow( tan(u / 2. + s45) , 1. / alfa)  *
                                            pow( (1. + e * sin(fi1)) / (1. - e * sin(fi1)) , e / 2.)
                                           )  - s45);

                      if (fabs(fi1 - lp_lat) < 0.000000000000001) ok=1;
                      fi1 = lp_lat;

                   }
                   while (ok==0);

                   lp_lon -= this->m_par.lam0;
                }

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

            };

            // Krovak
            template <typename Parameters, typename T>
            inline void setup_krovak(Parameters& par, par_krovak<T>& proj_parm)
            {
                T ts;
                /* read some Parameters,
                 * here Latitude Truescale */

                ts = pj_param(par.params, "rlat_ts").f;
                proj_parm.C_x = ts;

                /* we want Bessel as fixed ellipsoid */
                par.a = 6377397.155;
                par.e = sqrt(par.es = 0.006674372230614);

                    /* if latitude of projection center is not set, use 49d30'N */
                if (!pj_param(par.params, "tlat_0").i)
                        par.phi0 = 0.863937979737193;

                    /* if center long is not set use 42d30'E of Ferro - 17d40' for Ferro */
                    /* that will correspond to using longitudes relative to greenwich    */
                    /* as input and output, instead of lat/long relative to Ferro */
                if (!pj_param(par.params, "tlon_0").i)
                        par.lam0 = 0.7417649320975901 - 0.308341501185665;

                    /* if scale not set default to 0.9999 */
                if (!pj_param(par.params, "tk").i)
                        par.k0 = 0.9999;

                /* always the same */
            }

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

    /*!
        \brief Krovak projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Ellipsoid
        \par Projection parameters
         - lat_ts: Latitude of true scale (degrees)
         - lat_0: Latitude of origin
         - lon_0: Central meridian
         - k: Scale factor on the pseudo standard parallel
        \par Example
        \image html ex_krovak.gif
    */
    template <typename CalculationType, typename Parameters>
    struct krovak_ellipsoid : public detail::krovak::base_krovak_ellipsoid<CalculationType, Parameters>
    {
        inline krovak_ellipsoid(const Parameters& par) : detail::krovak::base_krovak_ellipsoid<CalculationType, Parameters>(par)
        {
            detail::krovak::setup_krovak(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::krovak, krovak_ellipsoid, krovak_ellipsoid)

        // Factory entry(s)
        template <typename CalculationType, typename Parameters>
        class krovak_entry : public detail::factory_entry<CalculationType, Parameters>
        {
            public :
                virtual base_v<CalculationType, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<krovak_ellipsoid<CalculationType, Parameters>, CalculationType, Parameters>(par);
                }
        };

        template <typename CalculationType, typename Parameters>
        inline void krovak_init(detail::base_factory<CalculationType, Parameters>& factory)
        {
            factory.add_to_factory("krovak", new krovak_entry<CalculationType, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_KROVAK_HPP


/*

RPBA - Robust Parallel Bundle Adjustment

File rpba.cpp

Description: Input controlled by file "parameters", call of parallel bundle adjustment and output

Copyright 2019 Helmut Mayer, Bundeswehr University Munich, Germany, Helmut.Mayer@unibw.de

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.

*/

#include <cstdio>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

#include "rpba.h"
#include "system.h"
#include <iomanip>      // std::setprecision
#include <fstream>      // std::ofstream

using std::string;
using std::ifstream;
using std::ofstream;
using namespace Eigen;

void ComputeRotationMatrixfromRodriguesAngle(const double a, const double b, const double c,
					     PMat &Rabc);
void ComputeRodriguesAnglefromRotationMatrix(double &a, double &b, double &c,
		const PMat &Rabc);

int main(int argc, char** argv) {
	// load parameters from parameter file
	if( argc > 1 ) {
		std::cout << "Parameter file not used\n";
		return 1;
	}

	boost::property_tree::ptree pt;
	boost::property_tree::ini_parser::read_ini("parameters", pt);

	std::string ID = pt.get<std::string>("InputDir");
	std::string File = pt.get<std::string>("File");
	std::string rf = pt.get<std::string>("Robust");
	std::string ee = pt.get<std::string>("Exteval");
	std::string k2i = pt.get<std::string>("k2");
	std::string k4i = pt.get<std::string>("k4");
	std::string fl = pt.get<std::string>("f");
	std::string k24outi = pt.get<std::string>("k24out");
	std::string mti = pt.get<std::string>("Threads");

	int mt = 0;
	bool k2f, k4f, flf, extevalflag, robustflag, k24outflag;

	std::stringstream ees(ee), rfs(rf), k2s(k2i), k4s(k4i), fls(fl), k24outs(k24outi), mts(mti), inputfiles, outputfiles;

	ees >> extevalflag;
	rfs >> robustflag;

	k2s >> k2f;
	k4s >> k4f;
	fls >> flf;

	k24outs >> k24outflag;

	mts >> mt;

	inputfiles << ID << "/" << File;
	std::string inputfile = inputfiles.str();

	std::cout << "Inputfile: " << inputfile << std::endl << std::endl;
	std::cout << "Robustflag: " << robustflag << std::endl << std::endl;

	if (mt < 1)
		mt = System::threadCount();

	std::cout << "Threads: " << mt << std::endl << std::endl;

	std::cout << "K2: " << k2f << std::endl;
	std::cout << "K4: " << k4f << std::endl;
	std::cout << "Focal length: " << flf << std::endl << std::endl;
	std::cout << "K24out: " << k24outflag << std::endl << std::endl;

	std::vector<bool> addflags(7,false);
	addflags[0] = k2f; // k2 - second order coefficient for radial distortion
	addflags[1] = k4f; // k4 - fourth order coefficient for radial distortion
	addflags[2] = flf; // f - focal length or photogrammetric camera constant

	int nraddpar = 0;
	for (int i = 0; i < 3; ++i)
		if (addflags[i])
			++nraddpar;

	MatrixXf XX;
	std::vector< Eigen::MatrixXf > xg;
	std::vector<std::vector<float> > wx, wy, wxy;
	std::vector<std::vector<IMAPNR> > pointXXv;
	std::vector< Eigen::Matrix<float, 3, 4> > PMatrices;
	std::vector<Camera> cameras;
	std::vector<int> iwidth, iheight;

	InputBlock(inputfile, nraddpar, XX, xg,
			wx, wy, wxy,
			pointXXv, PMatrices, cameras,
			iwidth, iheight,
			k24outflag);

	std::ostringstream os;
	const System::Time start = System::getTickCount();

	CalcPLSA(XX, xg,
			wx, wy, wxy,
			pointXXv, PMatrices, cameras,
			iwidth, iheight,
			robustflag,
			addflags,
			os, mt, extevalflag);

	if ((int) PMatrices.size() < 50 || mt == 1)
		std::cout << os.str();

	std::cout << "\n\nBundle adjustment runtime: " << (float) (System::getTickCount()-start) / 1000.f << "s\n\n";

/*	if (!extevalflag) {
		outputfiles << ID << "/out-" << File;
		std::string outputfile = outputfiles.str();

		OutputBlock(outputfile, XX, xg, pointXXv, PMatrices,
				cameras, k24outflag);
	} // if extevalflag */

/*
	// Output of text file with 3D coordinates
	std::stringstream cloudfiles;
	cloudfiles << ID << "/" << "cloud.txt";
	std::string cloudfile = cloudfiles.str();
	std::cout << "Outputfile: " << cloudfile << std::endl;
	ofstream foutc(cloudfile);
	foutc << std::setprecision(8);
	int counter = 0;
	for (int i = 0; i < XX.cols(); ++i) {
		if (pointXXv[i].size() > 0)
			foutc << XX(0,i) << ' ' << XX(1,i) << ' ' << XX(2,i) << '\n';
		else
			++counter;
	}
	foutc.close();

	// Counter gives number of 3D points which have been deleted during (robust) bundle adjustment
	std::cout << "Counter: " << counter << std::endl;
*/

	return 0;
}

void InputBlock(const std::string &inputfile, const int nraddpar,
		Eigen::MatrixXf &XX,
		std::vector<Eigen::MatrixXf> &xg,
		std::vector<std::vector<float> > &wx, std::vector<std::vector<float> > &wy,
		std::vector<std::vector<float> > &wxy,
		std::vector<std::vector<IMAPNR> > &pointXXv,
		std::vector<PMat> &PMatrices,
		std::vector<Camera> &cameras,
		std::vector<int> &iwidth, std::vector<int> &iheight,
		const bool k24outflag)
{
	ifstream fin(inputfile);

	int nrcameras,nrpoints,nrobs;

	fin >> nrcameras;
	fin >> nrpoints;
	fin >> nrobs;

	std::cout << "Cameras: " << nrcameras << "  Points: " << nrpoints << " Observations " << nrobs << std::endl;

	int nrobs2 = nrobs * 2;
	int nrparimages = (nrcameras - 2) * 6 + 5 + nrcameras * nraddpar;
	int nrunknowns = 3 * nrpoints + nrparimages;
	int redundancy = nrobs2 - nrunknowns;
	std::cout << "Unknowns " << nrunknowns << "  Redundancy " << redundancy << std::endl;

	std::vector<std::vector<float> > xin(nrcameras),yin(nrcameras);

	wx.resize(nrcameras);
	wy.resize(nrcameras);
	wxy.resize(nrcameras);

	pointXXv.resize(nrpoints);

	std::vector<float> minx(nrcameras,1.e20f),miny(nrcameras,1.e20f),maxx(nrcameras,-1.e20f),maxy(nrcameras,-1.e20f);
	int nrcam,nrpoint;
	float x,y;
	IMAPNR point;
	for (int i = 0; i < nrobs; ++i) {
		fin >> nrcam >> nrpoint >> x >> y;
		if (x < minx[nrcam])
			minx[nrcam] = x;
		if (y < miny[nrcam])
			miny[nrcam] = y;
		if (x > maxx[nrcam])
			maxx[nrcam] = x;
		if (y > maxy[nrcam])
			maxy[nrcam] = y;
		xin[nrcam].push_back(x);
		yin[nrcam].push_back(y);
		// set weight to unit matrix
		wx[nrcam].push_back(1.f);
		wy[nrcam].push_back(1.f);
		wxy[nrcam].push_back(0.f);
		point.image = nrcam;
		point.pointnr = (int) xin[nrcam].size() - 1;
		pointXXv[nrpoint].push_back(point);
	}

	PMatrices.resize(nrcameras);
	cameras.resize(nrcameras);
	xg.resize(nrcameras);
	iwidth.resize(nrcameras);
	iheight.resize(nrcameras);

	double a, b, c;
	float f, k2, k4;
	for (int i = 0; i < nrcameras; ++i) {
		cameras[i] = Camera();
		cameras[i].K.setZero();
		fin >> a >> b >> c >> PMatrices[i](0,3) >> PMatrices[i](1,3) >> PMatrices[i](2,3) >> f >> k2 >> k4;
		ComputeRotationMatrixfromRodriguesAngle(a,b,c,PMatrices[i]);
		cameras[i].K(1,1) = cameras[i].K(0,0) = f;
		cameras[i].K(2,2) = 1.f;
		if (k24outflag) {
			cameras[i].distpara[0] = k2;
			cameras[i].distpara[1] = k4;
		} // if
		else {
			cameras[i].distpara[0] = k2 * f * f;
			cameras[i].distpara[1] = k4 * f * f * f * f;
		} // else

		iwidth[i] = 1;
		iheight[i] = 1;

		xg[i].resize(3,(int) xin[i].size());
		for (int j = 0; j < (int) xin[i].size(); ++j) {
			xg[i](0,j) = -xin[i][j];
			xg[i](1,j) = -yin[i][j];
			xg[i](2,j) = 1.f;
		} // for j
	} // for i

	XX.resize(4,nrpoints);
	for (int i = 0; i < nrpoints; ++i) {
		fin >> XX(0,i) >> XX(1,i) >> XX(2,i);
		XX(3,i) = 1.f;
	} // for i

	fin.close();

	return;
}

void OutputBlock(const std::string &outputfile,
		Eigen::MatrixXf &XX,
		std::vector<Eigen::MatrixXf> &xg,
		std::vector<std::vector<IMAPNR> > &pointXXv,
		std::vector<PMat> &PMatrices,
		std::vector<Camera> &cameras,
		const bool k24outflag)
{
	ofstream fout(outputfile);

	int nrobs = 0;

	for (int i = 0; i < (int) pointXXv.size(); ++i)
		nrobs += (int) pointXXv[i].size();

	fout << (int) PMatrices.size() << ' ';
	fout << (int) XX.cols() << ' ';
	fout << nrobs << '\n';

	SBI sortbimage;

	fout << std::setprecision(8);
	for (int i = 0; i < (int) pointXXv.size(); ++i) {
		sort(pointXXv[i].begin(), pointXXv[i].end(), sortbimage);
		for (int j = 0; j < (int) pointXXv[i].size(); ++j) {
			IMAPNR &point = pointXXv[i][j];
			fout << point.image << ' ' << i << ' ' << -xg[point.image](0,point.pointnr) << ' ' << -xg[point.image](1,point.pointnr) << '\n';
		} // for j
	} // for i

	double a, b, c;
	for (int i = 0; i < (int) PMatrices.size(); ++i) {
		ComputeRodriguesAnglefromRotationMatrix(a, b, c, PMatrices[i]);
		fout << a << ' ' << b << ' ' << c << ' ' << PMatrices[i](0,3) << ' ' << PMatrices[i](1,3) << ' ' << PMatrices[i](2,3) << ' ';
		fout << cameras[i].K(0,0) << ' ';
		if (k24outflag)
			fout << cameras[i].distpara[0] << ' ' << cameras[i].distpara[1] << '\n';
		else {
			const double f = cameras[i].K(0,0);
			const double f2 = 1./ (f * f);
			fout << cameras[i].distpara[0] * f2 << ' ' << cameras[i].distpara[1] * f2 * f2 << '\n';
		}
	} // for i

	for (int i = 0; i < XX.cols(); ++i)
		fout << XX(0,i) << ' ' << XX(1,i) << ' ' << XX(2,i) << '\n';

	fout.close();

	return;
} // OutputBlock

// inspired by PBA
void ComputeRotationMatrixfromRodriguesAngle(const double a, const double b, const double c,
		PMat &Rabc){

	if (boost::math::isnan(a) || boost::math::isnan(b) || boost::math::isnan(c))
		std::cout << "NANRabc: " << a << ' ' << b << ' ' << c << '\n';

	const double a2 = a * a, b2 = b * b, c2 = c * c;
	const double ab = a * b, ac = a * c, bc = b * c;

	const double aa = sqrt(a2 + b2 + c2);
	const double ct = aa == 0.?0.5:(1. - cos(aa)) / (aa * aa);
	const double st = aa == 0.?1:sin(aa) / aa;
	Rabc(0,0) = (float) (1. - (b2 + c2) * ct);
	Rabc(0,1) = (float) (ab * ct - c * st);
	Rabc(0,2) = (float) (ac * ct + b * st);
	Rabc(1,0) = (float) (ab * ct + c * st);
	Rabc(1,1) = (float) (1. - (c2 + a2) * ct);
	Rabc(1,2) = (float) (bc * ct - a * st);
	Rabc(2,0) = (float) (ac * ct - b * st);
	Rabc(2,1) = (float) (bc * ct + a * st);
	Rabc(2,2) = (float) (1. - (a2 + b2) * ct);

	return;
} // ComputeRotationMatrixfromRodriguesAngle

// inspired by PBA
void ComputeRodriguesAnglefromRotationMatrix(double &a, double &b, double &c,
		const PMat &Rabc)
{
    const double epsilon0 = 0.01;
    const double epsilon1 = 0.1;
    const double pi = 3.14159265358979323846;

    double trace1 = (Rabc(0,0) + Rabc(1,1) + Rabc(2,2) - 1.) * 0.5;
    if (fabs(Rabc(0,1) - Rabc(1,0)) < epsilon0 &&
    		fabs(Rabc(1,2) - Rabc(2,1)) < epsilon0 &&
		fabs(Rabc(0,2) - Rabc(2,0)) < epsilon0 ) {
    	 	 if (fabs(Rabc(0,1) + Rabc(1,0)) < epsilon1 &&
    	 		fabs(Rabc(1,2) + Rabc(2,1)) < epsilon1 &&
			fabs(Rabc(0,2) + Rabc(2,0)) < epsilon1 && trace1 > 0.9) {
    	 		 a = b = c = 0.;
    	 		 return;
    	 	 }

    	 	 const double xx12 = (Rabc(0,0) + 1.) * 0.5;
    	 	 const double yy12 = (Rabc(1,1) + 1.) * 0.5;
    	 	 const double zz12 = (Rabc(2,2) + 1.) * 0.5;
    	 	 const double xy4 = (Rabc(0,1) + Rabc(1,0)) * 0.25;
    	 	 const double xz4 = (Rabc(0,2) + Rabc(2,0)) * 0.25;
    	 	 const double yz4 = (Rabc(1,2) + Rabc(2,1)) * 0.25;

    	 	 if ((xx12 > yy12) && (xx12 > zz12)) {
    	 		 if (xx12 < epsilon0) {
    	 			 a = 0.;
    	 			 b = c = sqrt(0.5) * pi;
    	 			 return;
    	 		 } // if xx
    	 		 const double t = sqrt(xx12);
    	 		 a = t * pi;
    	 		 b = xy4 / t * pi;
    	 		 c = xz4 / t * pi;
    	 		 return;
    	 	 } // if xx
    	 	 if (yy12 > zz12) {
    	 		 if (yy12 < epsilon0) {
    	 			 a = c = sqrt(0.5) * pi;
    	 			 b = 0.;
    	 			 return;
    	 		 } // if yy
    	 		 const double t = sqrt(yy12);
    	 		 a = xy4 / t * pi;
    	 		 b = t * pi;
    	 		 c = yz4 / t * pi;
    	 		 return;
    	 	 } // if
    	 	 if (zz12 < epsilon0) {
    	 		 a = b = sqrt(0.5) * pi;
    	 		 c = 0.;
    	 		 return;
    	 	 } // if zz
    	 	 const double t  = sqrt(zz12);
    	 	 a = xz4 / t * pi;
    	 	 b = yz4 / t * pi;
    	 	 c = t * pi;
    	 	 return;
    } // if fabs

    const double aa = acos(trace1);
    const double bb = 0.5 * aa / sin(aa);
    a = bb * (Rabc(2,1) - Rabc(1,2));
    b = bb * (Rabc(0,2) - Rabc(2,0));
    c = bb * (Rabc(1,0) - Rabc(0,1));

    return;
} // ComputeRodriguesAnglefromRotationMatrix


#include "problemes.h"
#include "arithmetique.h"
#include "timer.h"

#include <boost/bimap.hpp>
#include <boost/foreach.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

namespace {
    class Etat {
    public:
        Etat() {}

        std::deque<unsigned short> memoire_larry;
        std::deque<unsigned short> memoire_robin;

        short suivant(const unsigned short tirage) {
            short score = 0;
            // Larry's strategy is to remove the number that hasn't been called
            // in the longest time.
            auto it = std::find(memoire_larry.begin(), memoire_larry.end(), tirage);
            if (it != memoire_larry.end()) {
                ++score;
                memoire_larry.erase(it);
                memoire_larry.push_front(tirage);
            } else {
                memoire_larry.push_front(tirage);
                if (memoire_larry.size() > 5)
                    memoire_larry.pop_back();
            }

            // Robin's strategy is to remove the number that's been in the memory
            // the longest time.
            it = std::find(memoire_robin.begin(), memoire_robin.end(), tirage);
            if (it != memoire_robin.end()) {
                --score;
            } else {
                memoire_robin.push_front(tirage);
                if (memoire_robin.size() > 5)
                    memoire_robin.pop_back();
            }

            std::map<unsigned short, unsigned short> echange;
            unsigned short compteur = 0;
            for (auto &n: memoire_robin) {
                if (echange.find(n) == echange.end())
                    echange[n] = ++compteur;

                n = echange[n];
            }

            for (auto &n: memoire_larry) {
                if (echange.find(n) == echange.end())
                    echange[n] = ++compteur;

                n = echange[n];
            }

            return score;
        }

        bool operator<(const Etat &e) const {
            if (memoire_larry != e.memoire_larry)
                return memoire_larry < e.memoire_larry;

            return memoire_robin < e.memoire_robin;
        }
    };

    // std::ostream & operator<<(std::ostream & os, const Etat & e)
    // {
    //     os << "Larry " << e.memoire_larry << ", Robin " << e.memoire_robin;
    //     return os;
    // }
}

ENREGISTRER_PROBLEME(298, "Selective Amnesia") {
    // Larry and Robin play a memory game involving of a sequence of random numbers between 1 and 10, inclusive, that
    // are called out one at a time.
    // Each player can remember up to 5 previous numbers. When the called number is in a player's memory, that player is
    // awarded a point. If it's not, the player adds the called number to his memory, removing another number if his
    // memory is full.
    //
    // Both players start with empty memories. Both players always add new missed numbers to their memory but use a
    // different strategy in deciding which number to remove:
    //      - Larry's strategy is to remove the number that hasn't been called in the longest time.
    //      - Robin's strategy is to remove the number that's been in the memory the longest time.
    //
    // Example game:
    //
    //      Turn	Called 	    Larry's Larry's     Robin's Robin's
    //              number      memory  score       memory  score
    //         1	     1	          1	      0	          1	      0
    //         2	     2	        1,2	      0	        1,2	      0
    //         3	     4	      1,2,4	      0	      1,2,4	      0
    //         4	     6	    1,2,4,6	      0	    1,2,4,6	      0
    //         5	     1	    1,2,4,6	      1	    1,2,4,6	      1
    //         6	     8	  1,2,4,6,8	      1	  1,2,4,6,8	      1
    //         7	    10	 1,4,6,8,10	      1	 2,4,6,8,10	      1
    //         8	     2	 1,2,6,8,10	      1	 2,4,6,8,10	      2
    //         9	     4	 1,2,4,8,10	      1	 2,4,6,8,10	      3
    //        10	     1	 1,2,4,8,10	      2	 1,4,6,8,10	      3
    //
    // Denoting Larry's score by L and Robin's score by R, what is the expected value of |L-R| after 50 turns? Give your
    // answer rounded to eight decimal places using the format x.xxxxxxxx.
    typedef boost::bimap<Etat, unsigned short> bimap_t;
    typedef std::tuple<unsigned short/*etat*/, short/*score*/, long double/*probabilite*/> tuple_t;

    std::map<unsigned short/*etat*/, std::deque<tuple_t>> graphe;

    unsigned short compteur = 0;
    bimap_t relation;
    relation.insert(bimap_t::value_type(Etat(), compteur++));

    std::set<Etat> a_voir{Etat()};
    while (!a_voir.empty()) {
        const auto it = a_voir.begin();
        const unsigned short origine = relation.left.find(*it)->second;

        std::deque<tuple_t> suivants;

        for (unsigned short tirage = 0; tirage < 10; ++tirage) {
            Etat nouvel_etat = *it;
            short score = nouvel_etat.suivant(tirage);

            unsigned short suivant = 0;

            if (auto it2 = relation.left.find(nouvel_etat);it2 != relation.left.end()) {
                suivant = it2->second;
            } else {
                suivant = compteur;
                relation.insert(bimap_t::value_type(nouvel_etat, compteur++));
                a_voir.insert(nouvel_etat);
            }

            bool found = false;
            for (auto&[etat, s, probabilite]: suivants) {
                if (etat == suivant && s == score) {
                    probabilite += 0.1L;
                    found = true;
                }
            }

            if (!found) {
                suivants.emplace_back(suivant, score, 0.1);
            }
        }

        graphe.emplace(origine, suivants);
        a_voir.erase(it);
    }

    std::deque<std::map<short/*score*/, long double/*probabilite*/>> dp(compteur);
    dp.front().emplace(0, 1.0);

    for (unsigned short tour = 0; tour < 50; ++tour) {
        std::deque<std::map<short/*score*/, long double/*probabilite*/>> suivant(compteur);
        for (unsigned short etat = 0; etat < compteur; ++etat) {
            const auto &etats = graphe[etat];
            for (auto[score, probabilite]: dp[etat]) {
                for (const auto &[e/*etat*/, s/*score*/, p/*probabilite*/]: etats) {
                    suivant[e][score + s] += p * probabilite;
                }
            }
        }
        std::swap(suivant, dp);
    }

    long double resultat = 0.0L;
    for (const auto &p: dp)
        for (const auto &[score, probabilite]: p) {
            resultat += static_cast<long double>(std::abs(score)) * probabilite;
        }

    return std::to_fixed(resultat, 8);
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/Camera/CAHVORModel.h>
#include <fstream>
#include <boost/foreach.hpp>

using namespace vw;

// Overloaded constructor - this one reads in the file name
// where the CAVHOR camera model is saved.
camera::CAHVORModel::CAHVORModel(std::string const& filename) {

  try {
    std::ifstream input(filename.c_str(), std::ifstream::in);
    input.exceptions(std::ifstream::failbit | std::ifstream::badbit);

    vw_out(InfoMessage, "camera") << "Reading CAHVOR file: "
                                  << filename << ".\n";

    char r1,r2;

    while (true) {
      input.ignore(1024, 'C');
      input >> r1;
      if (r1 == '=')
        break;
    }

    input >> C(0) >> C(1) >> C(2);

    input >> r1 >> r2;
    if (r1 != 'A' || r2 != '=')
      vw_throw( IOErr() << "CAHVORModel: Could not read A vector\n" );
    input >> A(0) >> A(1) >> A(2);

    input >> r1 >> r2;
    if (r1 != 'H' || r2 != '=')
      vw_throw( IOErr() << "CAHVORModel: Could not read H vector\n" );
    input >> H(0) >> H(1) >> H(2);

    input >> r1 >> r2;
    if (r1 != 'V' || r2 != '=')
      vw_throw( IOErr() << "CAHVORModel: Could not read V vector\n" );
    input >> V(0) >> V(1) >> V(2);

    input >> r1 >> r2;
    if (r1 != 'O' || r2 != '=')
      vw_throw( IOErr() << "CAHVORModel: Could not read O vector\n" );
    input >> O(0) >> O(1) >> O(2);

    input >> r1 >> r2;
    if (r1 != 'R' || r2 != '=')
      vw_throw( IOErr() << "CAHVORModel: Could not read R vector\n" );
    input >> R(0) >> R(1) >> R(2);

  } catch (const std::ifstream::failure& e) {
    vw_throw( IOErr() << "CAHVORModel: Could not read file: " << filename << " (" << e.what() << ")" );
  }
}

// Write CAHVOR model to file.
void camera::CAHVORModel::write(std::string const& filename) {

  try {
    std::ofstream output(filename.c_str(), std::ofstream::out);
    output.exceptions(std::ofstream::failbit | std::ofstream::badbit);
    output.precision(20);

    vw_out(InfoMessage, "camera") << "Writing CAHVOR file: " << filename << "\n";

    output << "C = " << C(0) << " " << C(1) << " " << C(2) << "\n"
           << "A = " << A(0) << " " << A(1) << " " << A(2) << "\n"
           << "H = " << H(0) << " " << H(1) << " " << H(2) << "\n"
           << "V = " << V(0) << " " << V(1) << " " << V(2) << "\n"
           << "O = " << O(0) << " " << O(1) << " " << O(2) << "\n"
           << "R = " << R(0) << " " << R(1) << " " << R(2) << "\n";

  } catch (const std::ofstream::failure& e) {
    vw_throw( IOErr() << "CAHVORModel: Could not write file: " << filename << " (" << e.what() << ")" );
  }
}

// Set iteration and convergence constants
#define VW_CAHVOR_MAXITER  20     // maximum number of iterations allowed
#define VW_CAHVOR_CONV   1.0e-6   // covergence tolerance - check adequacy for application

// CAHVOR pixel_to_vector with partial_derivative output
Vector3 camera::CAHVORModel::pixel_to_vector(Vector2 const& pix,
                                             Matrix<double> &partial_derivatives) const {

  // Note, vec is actually the output vector
  // Based on JPL_CMOD_CAHVOR_2D_TO_3D
  Vector3 vec;
  int i, j;
  double omega, omega_2, tau, mu, u, u_2, du, k1, k3, k5, poly, deriv;

  Vector3 f, g, rr, pp, wo, lambda;

  Matrix3x3 m33, n33;
  double sgn, magv, magi;
  Vector3 t, w, v3, u3;
  Matrix3x3 irrt;

  double dudt;
  Vector3 drpdx, drpdy;
  Matrix3x3 dldr;

  Vector3 drdx, drdy;
  Matrix3x3 drpdr, drpdri;

  //  The projection point is merely the C of the camera model.
  //  vec = C; This isn't needed, only vec is returned for our
  //  usage.

  // Calculate the projection ray assuming normal vector directions,
  // neglecting distortion.

  f = pix.y() * A; // pos2[1] in JPL code is our y, and pos2[0] is x...
  f = V - f;

  g = pix.x() * A;
  g = H - g;
  rr = cross_prod(f,g);
  magi = 1.0/norm_2(rr);
  rr = magi * rr;

  // Check and optionally correct for vector directions.
  sgn = 1;
  t = cross_prod(V,H);

  if (dot_prod(t, A) < 0) {
    rr = -1.0 * rr;
    sgn = -1;
  }

  // Optionally compute partial for non-linear part of the model
  irrt.set_identity();

  for (i=0; i<3; i++) {
    for (j=0; j<3; j++) {
      irrt(i,j) -= (rr(i) * rr(j));
      // used to be A.as_ref() in vxl
      t = cross_prod(f, A);
      w = irrt * t;
      drpdx(0) = partial_derivatives(0,0) = -sgn * w(0) * magi;
      drpdx(1) = partial_derivatives(1,0) = -sgn * w(1) * magi;
      drpdx(2) = partial_derivatives(2,0) = -sgn * w(2) * magi;

      // used to be A.as_ref() in vxl
      t = cross_prod(g, A);
      w = irrt * t;
      drpdy(0) = partial_derivatives(0,1) = sgn * w(0) * magi;
      drpdy(1) = partial_derivatives(1,1) = sgn * w(1) * magi;
      drpdy(2) = partial_derivatives(2,1) = sgn * w(2) * magi;
    }
  }

  // Remove the radial lens distortion.  Preliminary values of
  // omega, lambda, and tau are computed from the rr vector
  // including distortion, in order to obtain the coefficients of
  // the equation k5*u^5 + k3*u^3 + k1*u = 1, which is solved for u
  // by means of Newton's method.  This value is used to compute the
  // corrected rr.
  omega = dot_prod(rr, O);
  omega_2 = omega * omega;
  wo = omega * O;
  lambda = rr - wo;
  tau = dot_prod(lambda, lambda) / omega_2;

  k1 = 1 + R(0);                //  1 + rho0
  k3 = R(1) * tau;              //  rho1*tau
  k5 = R(2) * tau*tau;  //  rho2*tau^2

  mu = R(0) + k3 + k5;
  u = 1.0 - mu; // initial approximation for iterations

  for (i=0; i<VW_CAHVOR_MAXITER; i++) {
    u_2 = u*u;
    poly  =  ((k5*u_2  +  k3)*u_2 + k1)*u - 1;
    deriv = (5*k5*u_2 + 3*k3)*u_2 + k1;

    if (deriv <= 0) {
      vw_out(InfoMessage, "camera") << "CAHVORModel.pixel_to_vector(): Distortion is too negative\n";
      break;
    } else {
      du = poly/deriv;
      u -= du;
      if (fabs(du) < VW_CAHVOR_CONV)
        break;
    }
  }

  if (i >= VW_CAHVOR_MAXITER) {
    vw_out(InfoMessage, "camera") << "CAHVORModel.pixel_to_vector(): Too many iterations (" << i << ")\n";
  }

  mu = 1 - u;
  pp = mu * lambda;
  vec = rr - pp;
  magv = norm_2(vec);
  vec = 1.0/magv * vec;

  // Note:  If partial derivatives are to be computed, corrected values
  // of omega, lambda, tau, and mu must be computed.

  // Recompute omega, lambda, tau, and mu
  omega = dot_prod(vec, O);
  omega_2 = omega * omega;
  wo = omega * O;
  lambda = vec - wo;
  tau = dot_prod(lambda, lambda) / omega_2;
  mu = R(0) + R(1)*tau + R(2)*tau*tau;

  // Compute the partial derivatives for distortion
  dldr.set_identity();

  Matrix<double, 3, 1> Ocol;
  Matrix<double, 1, 3> Orow;

  select_col(Ocol, 0) = O;
  select_row(Orow, 0) = O;

  m33 = Ocol*Orow; // outer product, hopefully
  dldr = dldr - m33;

  dudt = R(1) + (2 * R(2) * tau);

  v3 = transpose(transpose(lambda) * dldr);
  v3 = (2/omega_2) * v3;
  u3 = (2 * tau / omega) * O;
  v3 = v3 - u3;

  Matrix<double, 3, 1> lambdacol;
  Matrix<double, 1, 3> v3row;

  select_col(lambdacol,0) = lambda;
  select_row(v3row,0) = v3;

  m33 = lambdacol * v3row; // outer product, hopefully
  m33 = dudt * m33;
  n33 = mu * dldr;
  drpdr = m33 + n33;

  m33.set_identity();
  drpdr = m33 + drpdr;
  drpdr = magv * drpdr;

  // Apply these partials to get the final result
  drpdri = inverse(drpdr);
  drdx = drpdri * drpdx;
  drdy = drpdri * drpdy;

  partial_derivatives(0,0) = drdx(0);
  partial_derivatives(1,0) = drdx(1);
  partial_derivatives(2,0) = drdx(2);
  partial_derivatives(0,1) = drdy(0);
  partial_derivatives(1,1) = drdy(1);
  partial_derivatives(2,1) = drdy(2);

  return vec;
}

// pixel_to_vector (no returned partial matrix)
Vector3 camera::CAHVORModel::pixel_to_vector(Vector2 const& pix) const {
  // Based on JPL_CMOD_CAHVOR_2D_TO_3D

  // Calculate the projection ray assuming normal vector directions,
  // neglecting distortion.
  Vector3 rr = normalize(cross_prod(V - pix.y() * A,
                                    H - pix.x() * A));

  // Check and optionally correct for vector directions.
  if (dot_prod(cross_prod(V,H), A) < 0)
    rr = -1.0 * rr;

  // Remove the radial lens distortion.  Preliminary values of
  // omega, lambda, and tau are computed from the rr vector
  // including distortion, in order to obtain the coefficients of
  // the equation k5*u^5 + k3*u^3 + k1*u = 1, which is solved for u
  // by means of Newton's method.  This value is used to compute the
  // corrected rr.
  double omega = dot_prod(rr, O);
  Vector3 lambda = rr - omega * O;
  const double tau = dot_prod(lambda, lambda) / (omega*omega);

  const double k1 = 1 + R(0);        //  1 + rho0
  const double k3 = R(1) * tau;      //  rho1*tau
  const double k5 = R(2) * tau*tau;  //  rho2*tau^2

  double u = 1.0 - (R(0) + k3 + k5); // initial approximation for iterations

  double du, poly, deriv;
  for (int32 i=0;; i++) {
    if (i >= VW_CAHVOR_MAXITER) {
      vw_out(InfoMessage, "camera") << "CAHVORModel.pixel_to_vector(): Too many iterations (" << i << ")\n";
      break;
    }

    double u_2 = u*u;
    poly  =  ((k5*u_2  +  k3)*u_2 + k1)*u - 1;
    deriv = (5*k5*u_2 + 3*k3)*u_2 + k1;

    if (deriv <= 0) {
      vw_out(InfoMessage, "camera") << "CAHVORModel.pixel_to_vector(): Distortion is too negative\n";
      break;
    } else {
      du = poly/deriv;
      u -= du;
      if (fabs(du) < VW_CAHVOR_CONV)
        break;
    }
  }

  return normalize(rr - (1 - u)*lambda);
}

// vector_to_pixel with partial_derivatives
Vector2 camera::CAHVORModel::point_to_pixel(Vector3 const& point,
                                            Matrix<double> &partial_derivatives) const {

  Vector3 vec = point - C;
  // Based on JPL 3D to 2D POINT (not the 3D to 2D function alone).
  Vector2 pix;
  double alpha, beta, gamma, xh, yh;
  double omega, omega_2, tau, mu;
  Vector3 pp_c, wo, lambda;
  Matrix3x3 dldp, dppdp, m33, n33;
  Vector3 dxhdpp, dyhdpp, v3, u3;
  double dudt;

  // Calculate necessary quantities
  omega = dot_prod(vec,O);
  omega_2 = omega * omega;
  wo = omega * O;
  lambda = vec - wo;
  tau = dot_prod(lambda, lambda) / omega_2;
  mu = R(0) + (R(1) * tau) + (R(2) * tau * tau);
  pp_c = mu * lambda;
  pp_c = vec + pp_c;

  // Calculate alpha, beta, gamma,
  // dotted with a, h, v, respectively
  alpha = dot_prod(pp_c, A);
  beta = dot_prod(pp_c, H);
  gamma = dot_prod(pp_c, V);

  // Calculate the projection
  pix.x() = xh = beta / alpha;
  pix.y() = yh = gamma /alpha;

  // Calculate the approximate partial derivatives
  v3 = xh * A;
  v3 = H - v3;
  dxhdpp = 1/alpha * v3;
  v3 = yh * A;
  v3 = V - v3;
  dyhdpp = 1/alpha * v3;

  // Complete the calculations for accuracy
  dldp.set_identity();

  Matrix<double, 3, 1> Ocol;
  Matrix<double, 1, 3> Orow;

  select_col(Ocol, 0) = O;
  select_row(Orow, 0) = O;

  m33 = Ocol*Orow; // outer product, hopefully

  dldp = dldp - m33;
  dudt = R(1) + (2 * R(2) * tau);
  lambda = dldp * v3;
  v3 = 2/omega_2 * v3;
  u3 = (2 * tau/omega) * O;
  v3 = v3 - u3;

  Matrix<double, 3, 1> lambdacol;
  Matrix<double, 1, 3> v3row;

  select_col(lambdacol,0) = lambda;
  select_row(v3row,0) = v3;

  m33 = lambdacol * v3row; // outer product, hopefully

  m33 = dudt * m33;
  n33 = mu * dldp;
  dppdp = m33 + n33;
  m33.set_identity();
  dppdp = dppdp + m33;
  select_row(partial_derivatives,0) = transpose(transpose(dxhdpp) * dppdp);
  select_row(partial_derivatives,1) = transpose(transpose(dyhdpp) * dppdp);
  return pix;
}

// vector_to_pixel without partial_derivatives
Vector2 camera::CAHVORModel::point_to_pixel(Vector3 const& point) const {

  // Convert to directional
  Vector3 vec = point - C;

  // Calculate necessary quantities
  double omega = dot_prod(vec,O);
  Vector3 lambda = vec - omega * O;
  double tau = dot_prod(lambda,lambda) / (omega*omega);
  double mu = R(0) + (R(1) * tau) + (R(2) * tau * tau);
  Vector3 pp_c = vec + mu * lambda;

  double alpha = dot_prod(pp_c, A);

  // Calculate the projection
  return Vector2( dot_prod(pp_c,H) / alpha,
                  dot_prod(pp_c,V) / alpha );
}

// linearize_camera
//
// Takes CAHVOR camera --> CAHV camera
// Requires knowledge of size of image from CAHVOR camera
//
// This function warps a camera model so that it is purely
// linear. The parameter C will not change. The parameters O
// (identical to A) and R (all terms zero) will not be output. Note
// that image warping will be necessary in order to use the new
// models.
camera::CAHVModel camera::linearize_camera( camera::CAHVORModel const& camera_model,
                                            Vector2i const& cahvor_image_size,
                                            Vector2i const& cahv_image_size ) {

  CAHVModel output_camera;
  output_camera.C = camera_model.C;

  static const bool minfov = true; // set to 0 if you do not want to
                                   // minimize to a common field of
                                   // view

  // Record the landmark 2D coordinates around the perimeter of the image
  Vector2 hpts[6], vpts[6];
  hpts[0] = Vector2();
  hpts[1] = Vector2(0,(cahvor_image_size[1]-1)/2.0);
  hpts[2] = Vector2(0,cahvor_image_size[1]-1);
  hpts[3] = Vector2(cahvor_image_size[0]-1,0);
  hpts[4] = Vector2(cahvor_image_size[0]-1,(cahvor_image_size[1]-1)/2.0);
  hpts[5] = cahvor_image_size - Vector2(1,1);
  vpts[0] = Vector2();
  vpts[1] = Vector2((cahvor_image_size[0]-1)/2.0,0);
  vpts[2] = Vector2(cahvor_image_size[0]-1,0);
  vpts[3] = Vector2(0,cahvor_image_size[1]-1);
  vpts[4] = Vector2((cahvor_image_size[0]-1)/2,cahvor_image_size[1]-1);
  vpts[5] = cahvor_image_size - Vector2(1,1);

  BOOST_FOREACH( Vector2 const& local, vpts ) {
    output_camera.A += camera_model.pixel_to_vector(local);
  }
  BOOST_FOREACH( Vector2 const& local, hpts ) {
    output_camera.A += camera_model.pixel_to_vector(local);
  }
  output_camera.A = normalize(output_camera.A);

  // Compute the original right and down vectors
  Vector3 dn = cross_prod(camera_model.A, camera_model.H); // down vector
  Vector3 rt = normalize(cross_prod(dn, camera_model.A)); // right vector
  dn = normalize(dn);

  // Adjust the right and down vectors to be orthogonal to new axis
  rt = cross_prod(dn, output_camera.A);
  dn = normalize(cross_prod(output_camera.A, rt));
  rt = normalize(rt);

  // Find horizontal and vertical fields of view

  // Horizontal
  double hmin = 1, hmax = -1;
  BOOST_FOREACH( Vector2 const& loop, hpts ) {
    Vector3 u3 = camera_model.pixel_to_vector(loop);
    double sn = norm_2(cross_prod(output_camera.A,
                           normalize(u3 - dot_prod(dn, u3) * dn)));
    if (hmin > sn) hmin = sn;
    if (hmax < sn) hmax = sn;
  }

  // Vertical
  double vmin = 1, vmax = -1;
  BOOST_FOREACH( Vector2 const& loop, vpts ) {
    Vector3 u3 = camera_model.pixel_to_vector(loop);
    double sn = norm_2(cross_prod(output_camera.A,
                           normalize(u3 - dot_prod(rt, u3) * rt) ) );
    if (vmin > sn) vmin = sn;
    if (vmax < sn) vmax = sn;
  }

  // Compute the all-encompassing scale factors
  Vector2 scale_factors;
  Vector2 image_center = (cahv_image_size-Vector2(1,1))/2.0;
  Vector2 image_center_2 = elem_prod(image_center,image_center);
  if ( minfov ) {
    // Use min value
    scale_factors =
      sqrt( elem_quot(image_center_2,
                      Vector2(hmin*hmin,vmin*vmin)) - image_center_2 );
  } else {
    // Use max value
    scale_factors =
      sqrt( elem_quot(image_center_2,
                      Vector2(hmax*hmax,vmax*vmax)) - image_center_2 );
  }

  // Assign idealized image centers and coordinate angles
  output_camera.H = scale_factors[0] * rt + image_center[0] * output_camera.A;
  output_camera.V = scale_factors[1] * dn + image_center[1] * output_camera.A;

  return output_camera;
}

camera::CAHVModel camera::linearize_camera( CAHVORModel const& camera_model,
                                            int32 ix, int32 iy, int32 ox, int32 oy ) {
  return camera::linearize_camera( camera_model,
                                   Vector2i( ix, iy), Vector2i(ox, oy) );
}

// Note: the second derivatives with respect to motion of the point are the
// same as the second derivatives with respect to motion of the camera, and
// the first derivatives are opposite.
void camera::CAHVORModel::get_point_derivatives( Vector3 const& P, double& u, double& v,
                                                 Vector3& grad_u, Vector3& grad_v,
                                                 Matrix3x3& hess_u, Matrix3x3& hess_v ) const {
  // Compute the image plane position
  double xi = dot_prod(P-C,O);
  Vector3 lambda = P-C-xi*O;
  double tau = dot_prod(lambda,lambda)/(xi*xi);
  double mu = R[0]+tau*(R[1]+tau*R[2]);
  Vector3 PP = P + mu*lambda;
  Vector3 PPC = PP - C;
  double denom = dot_prod(PPC,A);
  u = dot_prod(PPC,H)/denom;
  v = dot_prod(PPC,V)/denom;

  // Compute the gradients
  Vector3 grad_tau = 2*(lambda-tau*xi*O)/(xi*xi);
  Vector3 grad_mu = 2*(R[1]+2*R[2]*tau)*(lambda/xi-tau*O)/xi;
  Vector3 HuA = H - u*A;
  Vector3 VvA = V - v*A;
  grad_u = ( (1+mu)*HuA - mu*dot_prod(O,HuA)*O + dot_prod(lambda,HuA)*grad_mu ) / denom;
  grad_v = ( (1+mu)*VvA - mu*dot_prod(O,VvA)*O + dot_prod(lambda,VvA)*grad_mu ) / denom;

  // Compute the Hessians
  Matrix3x3 I; I.set_identity();
  Matrix3x3 hess_tau = 2/(xi*xi) * ( I + (3*tau-1)*outer_prod(O,O)
                                     - 2*(outer_prod(O,lambda)+outer_prod(lambda,O))/xi );
  Matrix3x3 hess_mu = (R[1]+2*R[2]*tau)*hess_tau + 2*R[2]*outer_prod(grad_tau,grad_tau);
  Matrix3x3 tmp_u = outer_prod( grad_mu, HuA - dot_prod(O,HuA)*O )
    + outer_prod( grad_u, mu*dot_prod(O,A)*O - dot_prod(lambda,A)*grad_mu - (1+mu)*A );
  hess_u = ( tmp_u + transpose(tmp_u) + dot_prod(lambda,HuA)*hess_mu ) / denom;
  Matrix3x3 tmp_v = outer_prod( grad_mu, VvA - dot_prod(O,VvA)*O )
    + outer_prod( grad_v, mu*dot_prod(O,A)*O - dot_prod(lambda,A)*grad_mu - (1+mu)*A );
  hess_v = ( tmp_v + transpose(tmp_v) + dot_prod(lambda,VvA)*hess_mu ) / denom;
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the 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
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_GENERATION_HH
#define GRAPH_GENERATION_HH

#include <tuple>
#include <boost/functional/hash.hpp>
#include <map>
#include <set>
#include <iostream>

#include "graph_util.hh"
#include "random.hh"
#include "hash_map_wrap.hh"

namespace graph_tool
{
using namespace std;
using namespace boost;

// desired vertex type, with desired j,k values and the index in the real graph
struct dvertex_t
{
    dvertex_t(): in_degree(0), out_degree(0) {}
    dvertex_t(size_t in, size_t out): in_degree(in), out_degree(out) {}
    dvertex_t(const pair<size_t,size_t>& deg): in_degree(deg.first),
                                               out_degree(deg.second) {}
    size_t index, in_degree, out_degree;
    bool operator==(const dvertex_t& other) const {return other.index == index;}
};

size_t hash_value(const dvertex_t& v)
{
    return v.index;
}

// used for verbose display
void print_progress(size_t current, size_t total, stringstream& str)
{
    size_t atom = (total > 200) ? total/100 : 1;
    if ( ( (current+1) % atom == 0) || (current + 1) == total)
    {
        for (size_t j = 0; j < str.str().length(); ++j)
            cout << "\b";
        str.str("");
        str << current+1 << " of " << total << " ("
            << (current+1)*100 / total << "%)";
        cout << str.str() << flush;
    }
}

void print_update(size_t current, stringstream& str)
{
    for (size_t j = 0; j < str.str().length(); ++j)
        cout << "\b";
    for (size_t j = 0; j < str.str().length(); ++j)
        cout << " ";
    for (size_t j = 0; j < str.str().length(); ++j)
        cout << "\b";
    str.str("");
    str << current;
    cout << str.str() << flush;
}

//
// Generation strategies
// =====================
//
// Directed and undirected graphs have different generation strategies, which
// are defined in the classes below.

//
// Directed graph generation strategy
//

class DirectedStrat
{
public:
    typedef pair<size_t, size_t> deg_t; // degree type

    DirectedStrat(size_t N, bool no_parallel, bool no_self_loops)
        : _N(N), _no_parallel(no_parallel), _no_self_loops(no_self_loops)
    {
        if (_no_parallel)
            _max_deg = _no_self_loops ? _N - 1 : N;
        else
            _max_deg = numeric_limits<size_t>::max();
    }

    // check whether a degree sequence is graphical
    template <class DegSequence>
    bool is_graphical(DegSequence& d)
    {
        size_t one = _no_self_loops ? 1 : 0;

        size_t sum_out_deg = 0;
        size_t j = 0;
        for(auto di = d.begin(); di != d.end(); ++di)
        {
            size_t out_deg = di->first.second;
            size_t count = di->second;

            j += count;
            sum_out_deg += out_deg * count;

            size_t sum_in_deg = 0;
            auto dj_end = di; ++dj_end;
            for(auto dj = d.begin(); dj != dj_end; ++dj)
                sum_in_deg += min(dj->first.first, j - one) * dj->second;

            size_t sum_rest = 0;
            auto dj = di;
            for(++dj; dj != d.end(); ++dj)
                sum_rest += min(dj->first.first, j)*dj->second;

            if (sum_out_deg > sum_in_deg + sum_rest)
                return false;
        }
        return true;
    }

    // check whether a degree sequence is graphical, when parallel loops are
    // allowed but self-loops are not
    template <class DegSequence>
    bool is_graphical_parallel(DegSequence& d)
    {
        size_t sum_in_deg = 0;
        for(auto di = d.begin(); di != d.end(); ++di)
            sum_in_deg += di->first.first * di->second;
        for(auto di = d.begin(); di != d.end(); ++di)
        {
            if (di->first.second > sum_in_deg - di->first.first)
                return false;
        }
        return true;
    }

    // sample the degrees of all the vertices
    template <class DegSample>
    size_t SampleDegrees(vector<dvertex_t>& vertices, DegSample& deg_sample,
                         rng_t& rng, bool verbose)
    {
        stringstream str;
        size_t sum_j=0, sum_k=0;
        for(size_t i = 0; i < _N; ++i)
        {
            if (verbose)
                print_progress(i, _N, str);
            dvertex_t& v = vertices[i];
            do
            {
                tie(v.in_degree, v.out_degree) = deg_sample(i);
            }
            while (_no_parallel &&
                   (v.in_degree > _max_deg || v.out_degree > _max_deg));
            sum_j += v.in_degree;
            sum_k += v.out_degree;
            if (_no_parallel || _no_self_loops)
                _deg_seq[make_pair(v.in_degree, v.out_degree)]++;
        }

        if (verbose)
        {
            cout << "\nfixing average degrees. Total degree difference: "
                 << flush;
                str.str("");
        }

        // Sequence must be graphical. Re-sample random pairs until this holds
        uniform_int_distribution<size_t> vertex_sample(0, _N-1);
        size_t count = 0;
        while(sum_j != sum_k || (_no_parallel && !is_graphical(_deg_seq)) ||
              (_no_self_loops && !_no_parallel &&
               !is_graphical_parallel(_deg_seq)))
        {
            size_t i = vertex_sample(rng);
            dvertex_t& v = vertices[i];
            if (_no_parallel || _no_self_loops)
            {
                auto iter = _deg_seq.find(make_pair(v.in_degree, v.out_degree));
                iter->second--;
                if (iter->second == 0)
                    _deg_seq.erase(iter);
            }

            sum_j -= v.in_degree;
            sum_k -= v.out_degree;
            do
            {
                tie(v.in_degree, v.out_degree) = deg_sample(i);
            }
            while (_no_parallel &&
                   (v.in_degree > _max_deg || v.out_degree > _max_deg));
            sum_j += v.in_degree;
            sum_k += v.out_degree;
            if (_no_parallel || _no_self_loops)
                _deg_seq[make_pair(v.in_degree, v.out_degree)]++;
            if (verbose && (count % 100 == 0 || sum_j == sum_k))
                print_update(min(sum_j-sum_k, sum_k-sum_j), str);
            count++;
        }
        return sum_k;
    }

    struct deg_cmp
    {
        bool operator()(const deg_t& d1, const deg_t& d2) const
        {
            if (d1.second == d2.second)
                return d1.first > d2.first;
            return d1.second > d2.second;
        }
    };

private:
    size_t _N;
    bool _no_parallel;
    bool _no_self_loops;
    size_t _max_deg;
    map<deg_t, size_t, deg_cmp> _deg_seq;
};

//
// Undirected graph generation strategy
//

class UndirectedStrat
{
public:
    typedef size_t deg_t; // degree type

    UndirectedStrat(size_t N, bool no_parallel, bool no_self_loops)
        : _N(N), _no_parallel(no_parallel), _no_self_loops(no_self_loops)
    {
        if (_no_parallel)
            _max_deg = (_no_self_loops) ? _N - 1 : N;
        else
            _max_deg = numeric_limits<size_t>::max();
    }

    // check whether a degree sequence is graphical
    template <class DegSequence>
    bool is_graphical(DegSequence& d)
    {
        size_t one = (_no_self_loops) ? 1 : 0;
        size_t sum_deg = 0;
        size_t j = 0;
        for(auto di = d.begin(); di != d.end(); ++di)
        {
            j += di->second;
            sum_deg += di->first * di->second;
            size_t sum_rest = 0;
            auto dj = di;
            for(++dj; dj != d.end(); ++dj)
                sum_rest += min(dj->first, j)*dj->second;
            if (sum_deg > j*(j-one) + sum_rest)
                return false;
        }
        return true;
    }

    // check whether a degree sequence is graphical, when parallel loops are
    // allowed but self-loops are not
    template <class DegSequence>
    bool is_graphical_parallel(DegSequence& d)
    {
        size_t sum_deg = 0;
        for(auto& di : d)
            sum_deg += di.first * di.second;
        for(auto& di : d)
        {
            if (di.first > sum_deg - di.first)
                return false;
        }
        return true;
    }

    // samples the degress of all vertices
    template <class DegSample>
    size_t SampleDegrees(vector<dvertex_t>& vertices, DegSample& deg_sample,
                         rng_t& rng, bool verbose)
    {
        stringstream str;
        size_t sum_k=0;
        sum_k = 0;
        for(size_t i = 0; i < _N; ++i)
        {
            if (verbose)
                print_progress(i, _N, str);
            dvertex_t& v = vertices[i];
            do
            {
                v.out_degree = deg_sample(i, true);
            }
            while (_no_parallel && v.out_degree > _max_deg);
            sum_k += v.out_degree;
            _deg_seq[v.out_degree]++;
        }

        if (verbose)
        {
            cout << "\nFixing degree sequence: "
                 << flush;
            str.str("");
        }

        // sum_k must be an even number (2*num_edges), and degree sequence must
        // be graphical, if multiple edges are not allowed. Re-sample degrees
        // until this holds.
        uniform_int_distribution<size_t> vertex_sample(0, _N-1);
        size_t count = 0;
        while (sum_k % 2 != 0 || (_no_parallel && !is_graphical(_deg_seq)) ||
               (_no_self_loops && !_no_parallel &&
                !is_graphical_parallel(_deg_seq)))
        {
            size_t i = vertex_sample(rng);
            dvertex_t& v = vertices[i];
            if (_no_parallel || _no_self_loops)
            {
                auto iter = _deg_seq.find(v.out_degree);
                iter->second--;
                if(iter->second == 0)
                    _deg_seq.erase(iter);
            }
            sum_k -= v.out_degree;
            do
            {
                v.out_degree = deg_sample(i, true);
            }
            while (_no_parallel && (v.out_degree > _max_deg));
            sum_k +=  v.out_degree;
            if (_no_parallel || _no_self_loops)
                _deg_seq[v.out_degree]++;
            if (verbose && (count % 100 || sum_k % 2 == 0))
                print_update(sum_k, str);
            count++;
        }
        return sum_k/2;
    }

private:
    size_t _N;
    bool _no_parallel;
    bool _no_self_loops;
    size_t _max_deg;
    map<size_t, size_t, greater<size_t> > _deg_seq;
};

//
// Main Algorithm
// ==============
//
// generates a directed or undirected graph with given degree distribution

template <class Cmp>
struct cmp_in
{
    bool operator()(const pair<size_t,size_t>& v1,
                    const pair<size_t,size_t>& v2) const
    {
        if (v1.first == v2.first)
            return cmp(v1.second, v2.second);
        return cmp(v1.first, v2.first);
    }
    Cmp cmp;
};

template <class Cmp>
struct cmp_out
{
    bool operator()(const pair<size_t,size_t>& v1,
                    const pair<size_t,size_t>& v2) const
    {
        if (v1.second == v2.second)
            return cmp(v1.first, v2.first);
        return cmp(v1.second, v2.second);
    }
    Cmp cmp;
};

template <class Graph>
pair<size_t, size_t> get_deg(dvertex_t& v, Graph& g)
{
    return make_pair(v.in_degree - in_degreeS()(vertex(v.index, g), g),
                     v.out_degree - out_degree(vertex(v.index, g), g));
}

template <class Graph>
bool is_source(const pair<size_t, size_t>& deg)
{
    return deg.second > 0;
}

template <class Graph>
bool is_target(const pair<size_t, size_t>& deg)
{
    if (is_directed_::apply<Graph>::type::value)
        return deg.first > 0;
    else
        return is_source<Graph>(deg);
}

template <class Vset, class Targets, class Sources, class Graph>
bool update_deg(size_t t_i, const pair<size_t, size_t>& deg, Vset& vset,
                Targets& targets, Sources& sources, Graph&)
{
    if (is_source<Graph>(deg))
        sources.insert(deg);
    if (is_target<Graph>(deg))
        targets.insert(deg);
    vset[deg].push_back(t_i);
    return true;
}

struct gen_graph
{
    template <class Graph, class DegSample>
    void operator()(Graph& g, size_t N, DegSample& deg_sample, bool no_parallel,
                    bool no_self_loops, rng_t& rng, bool verbose, bool verify)
        const
    {
        typename property_map<Graph,vertex_index_t>::type vertex_index =
            get(vertex_index_t(), g);

        // figure out the necessary strategy
        typedef typename mpl::if_<typename is_directed_::apply<Graph>::type,
                                  DirectedStrat,
                                  UndirectedStrat>::type gen_strat_t;

        gen_strat_t gen_strat(N, no_parallel, no_self_loops);
        stringstream str; // used for verbose status

        if (verbose)
            cout << "adding vertices: " << flush;

        vector<dvertex_t> vertices(N);
        for(size_t i = 0; i < N; ++i)
            vertices[i].index = vertex_index[add_vertex(g)];

        // sample the N (j,k) pairs
        size_t E = gen_strat.SampleDegrees(vertices, deg_sample, rng, verbose);

        // source and target degree lists
        typedef pair<size_t, size_t> deg_t;
        set<deg_t, cmp_out<greater<size_t> > > sources;
        set<deg_t, cmp_in<greater<size_t> > > targets;

        // vertices with a given degree
        unordered_map<deg_t, vector<size_t>> vset; // can't use gt_hash_map, as
                                                   // internal pointers are
                                                   // invalidated after insert

        size_t num_e = 0;
        for (size_t i = 0; i < vertices.size();  ++i)
        {
            deg_t deg = get_deg(vertices[i], g);

            if (is_source<Graph>(deg))
                sources.insert(deg);
            if (is_target<Graph>(deg))
                targets.insert(deg);
            if (is_target<Graph>(deg) || is_source<Graph>(deg))
                vset[deg].push_back(i);
        }

        if (verbose)
        {
            cout << endl << "adding edges: " << flush;
            str.str("");
        }

        vector<size_t> skip;

        // connect edges: from sources with the largest in-degree to the ones
        // with largest out-degree
        while (!sources.empty())
        {
            // find source. The out-degree must be non-zero, and there must be a
            // vertex with the chosen degree.
            deg_t s_deg = *sources.begin();
            auto sv_iter = vset.find(s_deg);
            if (s_deg.second == 0 || sv_iter == vset.end() ||
                sv_iter->second.empty())
            {
                sources.erase(sources.begin());
                continue;
            }

            vector<size_t>& s_list = sv_iter->second;
            size_t s_i = s_list.front();
            typename graph_traits<Graph>::vertex_descriptor s =
                vertex(vertices[s_i].index, g);

            deg_t ns_deg = get_deg(vertices[s_i], g);
            if (ns_deg != s_deg)
            {
                swap(s_list.back(), s_list.front());
                s_list.pop_back();
                update_deg(s_i, ns_deg, vset, targets, sources, g);
                continue;
            }

            // find the targets.
            // we will keep an iterator to the current target degree
            auto t_iter = targets.begin();
            auto v_iter = vset.find(*t_iter);
            while (v_iter == vset.end() || v_iter->second.empty())
            {
                targets.erase(t_iter);
                t_iter = targets.begin();
                v_iter = vset.find(*t_iter);
            }

            skip.clear();
            skip.push_back(s_i);

            if (no_self_loops)
            {
                swap(s_list.back(), s_list.front());
                s_list.pop_back();
            }

            while (s_deg.second > 0)
            {
                // assert(!targets.empty());
                // assert(t_iter != targets.end());
                while (v_iter == vset.end() || v_iter->second.empty())
                {
                    ++t_iter;
                    v_iter = vset.find(*t_iter);
                }

                deg_t t_deg = *t_iter;

                vector<size_t>& v_list = v_iter->second;

                size_t t_i = v_list.front();

                deg_t nt_deg = get_deg(vertices[t_i], g);
                if (nt_deg != t_deg)
                {
                    swap(v_list.back(), v_list.front());
                    v_list.pop_back();
                    update_deg(t_i, nt_deg, vset, targets, sources, g);
                    t_iter = targets.begin();
                    v_iter = vset.find(*t_iter);
                    continue;
                }

                // remove target from vertex list, and get new t_i
                skip.push_back(t_i);

                swap(v_list.back(), v_list.front());
                v_list.pop_back();

                typename graph_traits<Graph>::vertex_descriptor t =
                    vertex(vertices[t_i].index, g);

                if ((s == t) && (!graph_tool::is_directed(g) &&
                                 s_deg.second < 2))
                    continue;

                add_edge(s, t, g);
                s_deg = get_deg(vertices[s_i], g);

                // if parallel edges are allowed, we should update the target
                // list right away
                if (!no_parallel)
                {
                    for (size_t i = 0; i < skip.size(); ++i)
                    {
                        if (no_self_loops && skip[i] == s_i)
                            continue;
                        update_deg(skip[i],
                                   get_deg(vertices[skip[i]], g), vset,
                                   targets, sources, g);
                        t_iter = targets.begin();
                        v_iter = vset.find(*t_iter);
                    }
                    skip.clear();
                    if (no_self_loops)
                        skip.push_back(s_i);
                }
                if (verbose)
                    print_progress(num_e++, E, str);
            }

            if (!s_list.empty() && s_list.front() == s_i)
            {
                swap(s_list.back(), s_list.front());
                s_list.pop_back();
            }

            // update modified degrees
            for (size_t i = 0; i < skip.size(); ++i)
                update_deg(skip[i],
                           get_deg(vertices[skip[i]], g),
                           vset, targets, sources, g);
        }
        if (verbose)
            cout << endl;

        if (verify)
        {
            for (size_t i = 0; i < vertices.size(); ++i)
            {
                deg_t dseq = make_pair(vertices[i].in_degree,
                                       vertices[i].out_degree);
                deg_t deg = make_pair(in_degreeS()(vertex(i, g), g),
                                      out_degree(vertex(i, g), g));
                if (deg != dseq)
                    throw GraphException("Graph does not match the desired "
                                         "sequence! Vertex " +
                                         lexical_cast<string>(i) +
                                         ", wanted: " +
                                         lexical_cast<string>(dseq.first) +
                                         " " +
                                         lexical_cast<string>(dseq.second) +
                                         ", got: " +
                                         lexical_cast<string>(deg.first) +
                                         " " +
                                         lexical_cast<string>(deg.second) +
                                         " This is a bug.");
            }
        }
    }
};

} // graph_tool namespace

#endif // GRAPH_GENERATION_HH


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <fstream>
#include <boost/math/special_functions/fpclassify.hpp>
#include <vinecopulib/misc/tools_stl.hpp>
#include <iostream>

namespace vinecopulib {

namespace tools_eigen {

//! remove rows of a matrix which contain nan values
//! @param x the matrix.
inline void remove_nans(Eigen::MatrixXd &x)
{
    // if a row has nan, move it to the end
    size_t last = x.rows() - 1;
    for (size_t i = 0; i < last + 1; i++) {
        if (x.row(i).array().isNaN().any())
            x.row(i--).swap(x.row(last--));
    }
    // remove nan rows
    x.conservativeResize(last + 1, x.cols());
}

//! remove rows of a matrix which contain nan values or have zero weight
//! @param x the matrix.
//! @param a vector of weights that is either empty or whose size is equal to
//!   the number of columns of x.
inline void remove_nans(Eigen::MatrixXd &x, Eigen::VectorXd &weights)
{
    if ((weights.size() > 0) & (weights.size() != x.rows()))
        throw std::runtime_error("sizes of x and weights don't match.");

    // if a row has nan or weight is zero, move it to the end
    size_t last = x.rows() - 1;
    for (size_t i = 0; i < last + 1; i++) {
        bool row_has_nan = x.row(i).array().isNaN().any();
        if (weights.size() > 0) {
            row_has_nan = row_has_nan | (boost::math::isnan)(weights(i));
            row_has_nan = row_has_nan | (weights(i) == 0.0);
        }
        if (row_has_nan) {
            if (weights.size() > 0)
                std::swap(weights(i), weights(last));
            x.row(i--).swap(x.row(last--));
        }
    }

    // remove nan rows
    x.conservativeResize(last + 1, x.cols());
    if (weights.size() > 0)
        weights.conservativeResize(last + 1);
}

//! check if all elements are contained in the unit cube.
//! @param u copula data.
//! @return `true` if all data lie in the unit cube; throws an error otherwise.
inline bool check_if_in_unit_cube(const Eigen::MatrixXd &u)
{
    bool any_outside = (u.array() < 0.0).any() | (u.array() > 1.0).any();
    if (any_outside) {
        throw std::runtime_error("all data must be contained in [0, 1]^d.");
    }
    return !any_outside;
}

//! swap the columns of a two-column matrix
//! @param u the matrix.
//! @return a new matrix v with `v.col(0) = u.col(1)`, `v.col(1) = u.col(0)`.
inline Eigen::Matrix<double, Eigen::Dynamic, 2> swap_cols(
    Eigen::Matrix<double, Eigen::Dynamic, 2> u)
{
    u.col(0).swap(u.col(1));
    return u;
}

//! computes the inverse \f$ f^{-1} \f$ of a function \f$ f \f$ by the
//! bisection method.
//!
//! @param x evaluation points.
//! @param f the function to invert.
//! @param lb lower bound.
//! @param ub upper bound.
//! @param n_iter the number of iterations for the bisection (defaults to 35,
//! guaranteeing an accuracy of 0.5^35 ~= 6e-11).
//!
//! @return \f$ f^{-1}(x) \f$.
inline Eigen::VectorXd invert_f(
    const Eigen::VectorXd &x,
    std::function<Eigen::VectorXd(const Eigen::VectorXd &)> f,
    const double lb,
    const double ub,
    int n_iter)
{
    Eigen::VectorXd xl = Eigen::VectorXd::Constant(x.size(), lb);
    Eigen::VectorXd xh = Eigen::VectorXd::Constant(x.size(), ub);
    Eigen::VectorXd x_tmp = x;
    Eigen::VectorXd fm(x.size());
    for (int iter = 0; iter < n_iter; ++iter) {
        x_tmp = (xh + xl) / 2.0;
        fm = f(x_tmp) - x;
        xl = (fm.array() < 0).select(x_tmp, xl);
        xh = (fm.array() < 0).select(xh, x_tmp);
    }
    if (fm.array().isNaN().any()) {
        size_t n = x.size();
        for (size_t j = 0; j < n; j++) {
            if ((boost::math::isnan)(fm(j))) {
                x_tmp(j) = std::numeric_limits<double>::quiet_NaN();
            }
        }
    }

    return x_tmp;
}

//! expand a vector into a matrix with two columns where each row
//! contains one combination of the vector elements
//!
//! @param grid_points the vector to expand.
inline Eigen::Matrix<double, Eigen::Dynamic, 2> expand_grid(
    const Eigen::VectorXd &grid_points)
{
    ptrdiff_t m = grid_points.size();
    Eigen::Matrix<double, Eigen::Dynamic, 2> grid_2d(m * m, 2);
    ptrdiff_t k = 0;
    for (ptrdiff_t i = 0; i < m; ++i) {
        for (ptrdiff_t j = 0; j < m; ++j) {
            grid_2d(k, 0) = grid_points(i);
            grid_2d(k, 1) = grid_points(j);
            ++k;
        }
    }
    return grid_2d;
}

//! reads data from a file to an Eigen matrix of integers.
//!
//! The function is currently **not safe** and may cause crashes when the
//! arguments are specified incorrectly.
//!
//! @param filename the name of the file to read from.
//! @param max_buffer_size the maximal buffer size.
inline Eigen::Matrix <size_t, Eigen::Dynamic, Eigen::Dynamic> read_matxs(
    const char *filename, int max_buffer_size)
{
    Eigen::MatrixXd temp = read_matxd(filename, max_buffer_size);
    Eigen::Matrix <size_t, Eigen::Dynamic, Eigen::Dynamic> output = temp.cast<size_t>();
    return output;
}

//! reads data from a file to an Eigen matrix of doubles.
//!
//! The function is currently **not safe** and may cause crashes when the
//! arguments are specified incorrectly.
//!
//! @param filename the name of the file to read from.
//! @param max_buffer_size the maximal buffer size.
inline Eigen::MatrixXd read_matxd(const char *filename, int max_buffer_size)
{
    using namespace std;

    int cols = 0, rows = 0;
    double *buff = new double[max_buffer_size];

    // Read numbers from file into buffer.
    ifstream infile;
    infile.open(filename);
    while (!infile.eof()) {
        string line;
        getline(infile, line);

        int temp_cols = 0;
        stringstream stream(line);
        while (!stream.eof()) {
            stream >> buff[cols * rows + temp_cols++];
        }
        if (temp_cols == 0) {
            continue;
        }
        if (cols == 0) {
            cols = temp_cols;
        }
        rows++;
    }

    infile.close();

    rows--;

    // Populate matrix with numbers.
    Eigen::MatrixXd result(rows, cols);
    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
            result(i, j) = buff[cols * i + j];
        }
    }

    delete[] buff;
    return result;
}

//! @}
}

}