#include "framework/builder/cfem_framework_builder.h"

#include <fstream>
#include <streambuf>
#include <deal.II/base/mpi.h>
#include <formulation/angular/self_adjoint_angular_flux.h>

#include "calculator/cell/integrated_fission_source.h"
#include "calculator/cell/total_aggregated_fission_source.h"
#include "convergence/moments/convergence_checker_l1_norm.hpp"
#include "convergence/reporter/mpi_noisy.h"
#include "domain/domain.hpp"
#include "domain/finite_element/finite_element_gaussian.h"
#include "domain/mesh/mesh_cartesian.hpp"
#include "eigenvalue/k_effective/updater_via_fission_source.h"
#include "formulation/cfem_diffusion_stamper.h"
#include "formulation/scalar/diffusion.hpp"
#include "formulation/cfem_saaf_stamper.h"
#include "framework/framework.h"
#include "iteration/updater/source_updater_gauss_seidel.h"
#include "iteration/updater/angular_source_updater_gauss_seidel.h"
#include "iteration/updater/fixed_updater.h"
#include "iteration/updater/angular_fixed_updater.h"
#include "iteration/initializer/initialize_fixed_terms_once.h"
#include "iteration/initializer/set_fixed_terms_once.h" // to be removed
#include "iteration/group/group_source_iteration.hpp"
#include "iteration/outer/outer_power_iteration.h"
#include "material/material_protobuf.h"
#include "problem/parameter_types.h"
#include "convergence/parameters/single_parameter_checker.h"
#include "convergence/final_checker_or_n.h"
#include "quadrature/utility/quadrature_utilities.h"
#include "quadrature/factory/quadrature_factories.h"
#include "results/output_dealii_vtu.h"
#include "solver/group/single_group_solver.h"
#include "solver/gmres.h"
#include "system/system.h"
#include "system/solution/mpi_group_angular_solution.h"
#include "system/terms/term.h"
#include "system/terms/term_types.h"
#include "system/moments/spherical_harmonic.h"
#include "system/system_functions.h"

namespace bart {

namespace framework {

namespace builder {

template <int dim>
std::unique_ptr<FrameworkI> CFEM_FrameworkBuilder<dim>::BuildFramework(
    problem::ParametersI &prm,
    dealii::ParameterHandler &d2_prm) {

  std::cout << "Setting up materials" << std::endl;

  std::ifstream mapping_file(prm.MaterialMapFilename());
  std::string material_mapping(
      (std::istreambuf_iterator<char>(mapping_file)),
      std::istreambuf_iterator<char>());

  const int n_groups = prm.NEnergyGroups();
  int n_angles;

  MaterialProtobuf materials(d2_prm);
  auto cross_sections_ptr = std::make_shared<bart::data::CrossSections>(materials);

  std::cout << "Building Finite Element"<< std::endl;
  std::shared_ptr<FiniteElement>finite_element_ptr(std::move(BuildFiniteElement(
      &prm)));

  std::cout << "Building Domain" << std::endl;
  std::shared_ptr<Domain> domain_ptr(std::move(BuildDomain(
      &prm,finite_element_ptr, material_mapping)));

  std::cout << "Setting up domain" << std::endl;
  domain_ptr->SetUpMesh().SetUpDOF();

  std::shared_ptr<SourceUpdater> source_updater_ptr;
  std::unique_ptr<Initializer> initializer_ptr;

  std::shared_ptr<AngularQuadratureSet> quadrature_ptr = nullptr;

  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    printf("Building Quadrature Set\n");
    quadrature_ptr = BuildAngularQuadratureSet(&prm);
    n_angles = quadrature_ptr->size();
  } else {
    printf("Scalar solve\n");
    n_angles = 1;
  }

  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    printf("Building Stamper\n");
    std::shared_ptr<CFEMAngularStamper> stamper_ptr(std::move(BuildAngularStamper(
        &prm, domain_ptr, finite_element_ptr, cross_sections_ptr, quadrature_ptr)));
    printf("Building Source Updater\n");
    source_updater_ptr = std::move(BuildSourceUpdater(&prm, stamper_ptr, quadrature_ptr));
    printf("Building Initializer\n");
    initializer_ptr = BuildInitializer(&prm, stamper_ptr, quadrature_ptr);
  } else {
    printf("Building Scalar Stamper\n");
    std::shared_ptr<CFEMStamper> stamper_ptr(std::move(BuildStamper(
        &prm, domain_ptr, finite_element_ptr, cross_sections_ptr)));
    printf("Building Source Updater\n");
    source_updater_ptr = std::move(BuildSourceUpdater(&prm, stamper_ptr));
    printf("Building Initializer\n");
    initializer_ptr = BuildInitializer(&prm, stamper_ptr);
  }

  std::cout << "Building single group solver" << std::endl;
  auto single_group_solver_ptr = BuildSingleGroupSolver();

  std::cout << "Building inner iteration objects" << std::endl;

  auto in_group_final_checker = BuildMomentConvergenceChecker(1e-10, 100);

  // Build reporter
  std::shared_ptr<ConvergenceReporter> reporter(std::move(BuildConvergenceReporter()));

  auto moment_calculator_type = quadrature::MomentCalculatorImpl::kScalarMoment;

  // Moment calculator
  if (prm.TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    moment_calculator_type = quadrature::MomentCalculatorImpl::kZerothMomentOnly;
  }

  auto moment_calculator_ptr = quadrature::factory::MakeMomentCalculator<dim>(
      moment_calculator_type,
      quadrature_ptr);

  std::cout << "Building and initializing system object" << std::endl;

  // Solution group
  auto solution_ptr =
      std::make_shared<system::solution::MPIGroupAngularSolution>(n_angles);

  system::SetUpMPIAngularSolution(*solution_ptr, *domain_ptr);

  std::cout << "Building in-group iteration" << std::endl;

  auto in_group_iteration =
      std::make_unique<iteration::group::GroupSourceIteration<dim>>(
          std::move(single_group_solver_ptr),
          std::move(in_group_final_checker),
          std::move(moment_calculator_ptr),
          solution_ptr,
          source_updater_ptr,
          reporter);

  std::cout << "Building K_Effective updater" << std::endl;

  // KEffectiveUpdater
  using FissionSourceCalulator = calculator::cell::TotalAggregatedFissionSource<dim>;
  using IntegratedFissionSourceCalc = calculator::cell::IntegratedFissionSource<dim>;
  auto int_fission_ptr = std::make_unique<IntegratedFissionSourceCalc>(
      finite_element_ptr, cross_sections_ptr);
  auto fission_source_calc_ptr = std::make_unique<FissionSourceCalulator>(
      std::move(int_fission_ptr), domain_ptr);
  using KEffectiveUpdater = eigenvalue::k_effective::UpdaterViaFissionSource;

  // KEffective convergence checker
  using KEffConvChecker = bart::convergence::parameters::SingleParameterChecker;
  auto k_eff_conv_checker = std::make_unique<KEffConvChecker>();
  using KEffectiveFinalCovergence = bart::convergence::FinalCheckerOrN<double,
      bart::convergence::parameters::SingleParameterChecker>;
  auto k_eff_final_checker = std::make_unique<KEffectiveFinalCovergence>(
      std::move(k_eff_conv_checker)
      );

  // KEffective updater
  auto k_effective_updater = std::make_unique<KEffectiveUpdater>(
      std::move(fission_source_calc_ptr), 2.0, 10);

  std::cout << "Building outer-iteration" << std::endl;

  using PowerIteration = iteration::outer::OuterPowerIteration;
  auto power_iteration_ptr = std::make_unique<PowerIteration>(
      std::move(in_group_iteration),
      std::move(k_eff_final_checker),
      std::move(k_effective_updater),
      source_updater_ptr,
      reporter
  );

  std::cout << "Building System" << std::endl;

  auto system = std::make_unique<system::System>();

  system->total_groups = n_groups;
  std::unordered_set<bart::system::terms::VariableLinearTerms>
      source_terms{bart::system::terms::VariableLinearTerms::kScatteringSource,
                   bart::system::terms::VariableLinearTerms::kFissionSource};
  system->right_hand_side_ptr_ =
      std::make_unique<system::terms::MPILinearTerm>(source_terms);
  system->left_hand_side_ptr_ =
      std::make_unique<system::terms::MPIBilinearTerm>();

  std::cout << "Filling system" << std::endl;

  // Fill system with objects
  for (int group = 0; group < n_groups; ++group) {
    for (int angle = 0; angle < n_angles; ++angle) {
      system::Index index{group, angle};
      // LHS
      auto fixed_matrix_ptr = domain_ptr->MakeSystemMatrix();
      system->left_hand_side_ptr_->SetFixedTermPtr(index, fixed_matrix_ptr);

      // RHS
      auto fixed_vector_ptr =
          std::make_shared<bart::system::MPIVector>(domain_ptr->locally_owned_dofs(),
                                                    MPI_COMM_WORLD);
      system->right_hand_side_ptr_->SetFixedTermPtr(index, fixed_vector_ptr);

      for (auto term : source_terms) {
        auto variable_vector_ptr =
            std::make_shared<bart::system::MPIVector>(domain_ptr->locally_owned_dofs(),
                                                      MPI_COMM_WORLD);
        system->right_hand_side_ptr_->SetVariableTermPtr(
            index, term, variable_vector_ptr);
      }
    }
  }

  std::cout << "Fill system moments" << std::endl;

  // Moments
  system->current_moments =
      std::make_unique<system::moments::SphericalHarmonic>(n_groups, 0);
  system->previous_moments =
      std::make_unique<system::moments::SphericalHarmonic>(n_groups, 0);

  for (auto& moment_pair : system->current_moments->moments()) {
    auto index = moment_pair.first;
    auto& current_moment = system->current_moments->operator[](index);
    current_moment.reinit(solution_ptr->operator[](0).size());
    auto& previous_moment = system->previous_moments->operator[](index);
    previous_moment.reinit(solution_ptr->operator[](0).size());
    current_moment = 1;
    previous_moment = 1;
  }

  // Initialize System
  system->k_effective = 1.0;
  system->total_groups = n_groups;
  system->total_angles = n_angles;

  std::cout << "Build Results Output" << std::endl;
  auto results_output_ptr =
      std::make_unique<results::OutputDealiiVtu<dim>>(domain_ptr);

  return std::make_unique<framework::Framework>(
      std::move(system),
      std::move(initializer_ptr),
      std::move(power_iteration_ptr),
      std::move(results_output_ptr));
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildAngularQuadratureSet(
        problem::ParametersI* problem_parameters)
-> std::shared_ptr<AngularQuadratureSet> {

  std::shared_ptr<AngularQuadratureSet> return_ptr = nullptr;

  std::shared_ptr<quadrature::QuadratureGeneratorI<dim>>
      quadrature_generator_ptr = nullptr;

  const int order_value = problem_parameters->AngularQuadOrder();
  switch (problem_parameters->AngularQuad()) {
    default: {
      if (dim == 3) {
        quadrature_generator_ptr =
            quadrature::factory::MakeAngularQuadratureGeneratorPtr<dim>(
                quadrature::Order(order_value),
                quadrature::AngularQuadratureSetType::kLevelSymmetricGaussian);
      } else {
        AssertThrow(false,
            dealii::ExcMessage("No supported quadratures for this dimension "
                               "and transport model"))
      }
    }
  }

  return_ptr = quadrature::factory::MakeQuadratureSetPtr<dim>();

  auto quadrature_points = quadrature::utility::GenerateAllPositiveX<dim>(
      quadrature_generator_ptr->GenerateSet());

  quadrature::factory::FillQuadratureSet<dim>(return_ptr.get(),
                                              quadrature_points);

  return std::move(return_ptr);
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildConvergenceReporter()
-> std::unique_ptr<ConvergenceReporter> {
  std::unique_ptr<ConvergenceReporter> return_ptr = nullptr;

  using Reporter = bart::convergence::reporter::MpiNoisy;
  int this_process = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  auto pout_ptr = std::make_unique<dealii::ConditionalOStream>(std::cout, this_process == 0);
  return_ptr = std::make_unique<Reporter>(std::move(pout_ptr));

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildDomain(
    problem::ParametersI *problem_parameters,
    const std::shared_ptr<FiniteElement> &finite_element_ptr,
    std::string material_mapping)-> std::unique_ptr<Domain> {

  // Build mesh
  auto mesh_ptr = std::make_unique<domain::mesh::MeshCartesian<dim>>(
      problem_parameters->SpatialMax(),
      problem_parameters->NCells(),
      material_mapping);

  return std::make_unique<domain::Definition<dim>>(
      std::move(mesh_ptr), finite_element_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildFiniteElement(
    problem::ParametersI *problem_parameters)-> std::unique_ptr<FiniteElement> {
  return std::make_unique<domain::finite_element::FiniteElementGaussian<dim>>(
      problem::DiscretizationType::kContinuousFEM,
      problem_parameters->FEPolynomialDegree());
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildInitializer(
    const problem::ParametersI *problem_parameters,
    const std::shared_ptr<CFEMStamper> &stamper_ptr)
-> std::unique_ptr<Initializer> {

  std::unique_ptr<Initializer> return_ptr = nullptr;

  using FixedUpdaterType = iteration::updater::FixedUpdater<CFEMStamper>;
  auto fixed_updater_ptr = std::make_unique<FixedUpdaterType>(stamper_ptr);

  if (problem_parameters->TransportModel() == problem::EquationType::kDiffusion) {
    return_ptr = std::make_unique<iteration::initializer::SetFixedTermsOnce>(
        std::move(fixed_updater_ptr), problem_parameters->NEnergyGroups(), 1);
  }

  return std::move(return_ptr);
}

template <int dim>
auto CFEM_FrameworkBuilder<dim>::BuildInitializer(
    const problem::ParametersI *problem_parameters,
    const std::shared_ptr<CFEMAngularStamper> &stamper_ptr,
    const std::shared_ptr<AngularQuadratureSet>& quadrature_set_ptr)
-> std::unique_ptr<Initializer> {

  std::unique_ptr<Initializer> return_ptr = nullptr;

  using FixedUpdaterType = iteration::updater::AngularFixedUpdater<CFEMAngularStamper>;
  auto fixed_updater_ptr = std::make_unique<FixedUpdaterType>(
      stamper_ptr, quadrature_set_ptr);

  if (problem_parameters->TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {
    return_ptr = std::make_unique<iteration::initializer::SetFixedTermsOnce>(
        std::move(fixed_updater_ptr),
        problem_parameters->NEnergyGroups(),
        quadrature_set_ptr->size());
  }

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildMomentConvergenceChecker(
    double max_delta, int max_iterations)
-> std::unique_ptr<MomentConvergenceChecker>{
  //TODO(Josh): Add option for using other than L1Norm

  using CheckerType = convergence::moments::SingleMomentCheckerL1Norm;
  using FinalCheckerType = convergence::FinalCheckerOrN<
      system::moments::MomentVector,
      convergence::moments::SingleMomentCheckerI>;

  auto single_checker_ptr = std::make_unique<CheckerType>(max_delta);
  auto return_ptr = std::make_unique<FinalCheckerType>(
      std::move(single_checker_ptr));

  return_ptr->SetMaxIterations(max_iterations);

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildParameterConvergenceChecker(
    double max_delta, int max_iterations)
-> std::unique_ptr<ParameterConvergenceChecker>{

  using CheckerType = convergence::parameters::SingleParameterChecker;
  using FinalCheckerType = convergence::FinalCheckerOrN<double, CheckerType>;

  auto single_checker_ptr = std::make_unique<CheckerType>(max_delta);
  auto return_ptr = std::make_unique<FinalCheckerType>(
      std::move(single_checker_ptr));

  return_ptr->SetMaxIterations(max_iterations);

  return std::move(return_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildSingleGroupSolver(
    const int max_iterations,
    const double convergence_tolerance) -> std::unique_ptr<SingleGroupSolver> {
  std::unique_ptr<SingleGroupSolver> return_ptr = nullptr;

  auto linear_solver_ptr = std::make_unique<solver::GMRES>(max_iterations,
                                                           convergence_tolerance);

  return_ptr = std::move(
      std::make_unique<solver::group::SingleGroupSolver>(
          std::move(linear_solver_ptr)));

  return return_ptr;
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildSourceUpdater(
    problem::ParametersI *,
    const std::shared_ptr<CFEMStamper> stamper_ptr)
    -> std::unique_ptr<SourceUpdater> {
  // TODO(Josh): Add option for non-gauss-seidel updating
  using SourceUpdater = iteration::updater::SourceUpdaterGaussSeidel<CFEMStamper>;
  return std::make_unique<SourceUpdater>(stamper_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildSourceUpdater(
    problem::ParametersI *,
    const std::shared_ptr<CFEMAngularStamper> stamper_ptr,
    const std::shared_ptr<AngularQuadratureSet>& quadrature_set_ptr)
-> std::unique_ptr<SourceUpdater> {
  // TODO(Josh): Add option for non-gauss-seidel updating
  using SourceUpdater = iteration::updater::AngularSourceUpdaterGaussSeidel<CFEMAngularStamper>;
  return std::make_unique<SourceUpdater>(stamper_ptr, quadrature_set_ptr);
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildStamper(
    problem::ParametersI *problem_parameters,
    const std::shared_ptr<Domain> &domain_ptr,
    const std::shared_ptr<FiniteElement> &finite_element_ptr,
    const std::shared_ptr<CrossSections> &cross_sections_ptr)
-> std::unique_ptr<CFEMStamper> {

  std::unique_ptr<CFEMStamper> return_ptr = nullptr;

  // Diffusion Stamper
  if (problem_parameters->TransportModel() == problem::EquationType::kDiffusion) {

    auto diffusion_ptr = std::make_unique<formulation::scalar::Diffusion<dim>>(
        finite_element_ptr, cross_sections_ptr);

    return_ptr = std::move(
        std::make_unique<formulation::CFEM_DiffusionStamper<dim>>(
            std::move(diffusion_ptr),
            domain_ptr,
            problem_parameters->ReflectiveBoundary()));

  } else {
    AssertThrow(false, dealii::ExcMessage("Unsuppored equation type passed"
                                          " to BuildScalarFormulation"));
  }

  return return_ptr;
}

template<int dim>
auto CFEM_FrameworkBuilder<dim>::BuildAngularStamper(
    problem::ParametersI *problem_parameters,
    const std::shared_ptr<Domain> &domain_ptr,
    const std::shared_ptr<FiniteElement> &finite_element_ptr,
    const std::shared_ptr<CrossSections> &cross_sections_ptr,
    const std::shared_ptr<AngularQuadratureSet>& quadrature_set_ptr)
-> std::unique_ptr<CFEMAngularStamper> {

  std::unique_ptr<CFEMAngularStamper> return_ptr = nullptr;

  if (problem_parameters->TransportModel() == problem::EquationType::kSelfAdjointAngularFlux) {

    auto saaf_formulation_ptr =
        std::make_unique<formulation::angular::SelfAdjointAngularFlux<dim>>(
        finite_element_ptr, cross_sections_ptr, quadrature_set_ptr);

    return_ptr = std::move(
        std::make_unique<formulation::CFEM_SAAF_Stamper<dim>>(
            std::move(saaf_formulation_ptr),
            domain_ptr));
  } else {
    AssertThrow(false, dealii::ExcMessage("Unsuppored equation type passed"
                                          "to BuildScalarFormulation"));
  }

  return return_ptr;
}

template class CFEM_FrameworkBuilder<1>;
template class CFEM_FrameworkBuilder<2>;
template class CFEM_FrameworkBuilder<3>;

} // namespace builder

} // namespace framework

} // namespace bart


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_DISTANCE_GEOGRAPHIC_HPP
#define BOOST_GEOMETRY_STRATEGIES_DISTANCE_GEOGRAPHIC_HPP

#include <boost/geometry/strategies/distance/comparable.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/distance/services.hpp>
#include <boost/geometry/strategies/detail.hpp>

#include <boost/geometry/strategies/geographic/azimuth.hpp>

#include <boost/geometry/strategies/geographic/distance.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_box_box.hpp>
#include <boost/geometry/strategies/geographic/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/geographic/distance_segment_box.hpp>
// TODO - for backwards compatibility, remove?
#include <boost/geometry/strategies/geographic/distance_andoyer.hpp>
#include <boost/geometry/strategies/geographic/distance_thomas.hpp>
#include <boost/geometry/strategies/geographic/distance_vincenty.hpp>

#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/relate/geographic.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace distance
{

// TODO: azimuth and normalize getters would not be needed if distance_segment_box was implemented differently
//       right now it calls disjoint algorithm details.

template
<
    typename FormulaPolicy = strategy::andoyer,
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
    : public strategies::relate::geographic<FormulaPolicy, SeriesOrder, Spheroid, CalculationType>
{
    using base_t = strategies::relate::geographic<FormulaPolicy, SeriesOrder, Spheroid, CalculationType>;

public:
    geographic() = default;

    explicit geographic(Spheroid const& spheroid)
        : base_t(spheroid)
    {}

    // azimuth

    auto azimuth() const
    {
        return strategy::azimuth::geographic
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // distance

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic
                <
                    FormulaPolicy, Spheroid, CalculationType
                >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_ps_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_cross_track
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_pb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_cross_track_point_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_sb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_segment_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_bb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::geographic_cross_track_box_box
            <
                FormulaPolicy, Spheroid, CalculationType
            >(base_t::m_spheroid);
    }

    // normalize

    template <typename Geometry>
    static auto normalize(Geometry const&,
                          std::enable_if_t
                            <
                                util::is_point<Geometry>::value
                            > * = nullptr)
    {
        return strategy::normalize::spherical_point();
    }
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy<Geometry1, Geometry2, geographic_tag, geographic_tag>
{
    using type = strategies::distance::geographic<>;
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic<FP, S, CT> >
{
    static auto get(strategy::distance::geographic<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};
// TODO - for backwards compatibility, remove?
template <typename S, typename CT>
struct strategy_converter<strategy::distance::andoyer<S, CT> >
{
    static auto get(strategy::distance::andoyer<S, CT> const& s)
    {
        return strategies::distance::geographic<strategy::andoyer, strategy::default_order<strategy::andoyer>::value, S, CT>(s.model());
    }
};
// TODO - for backwards compatibility, remove?
template <typename S, typename CT>
struct strategy_converter<strategy::distance::thomas<S, CT> >
{
    static auto get(strategy::distance::thomas<S, CT> const& s)
    {
        return strategies::distance::geographic<strategy::thomas, strategy::default_order<strategy::thomas>::value, S, CT>(s.model());
    }
};
// TODO - for backwards compatibility, remove?
template <typename S, typename CT>
struct strategy_converter<strategy::distance::vincenty<S, CT> >
{
    static auto get(strategy::distance::vincenty<S, CT> const& s)
    {
        return strategies::distance::geographic<strategy::vincenty, strategy::default_order<strategy::vincenty>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_cross_track<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_cross_track<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_cross_track_point_box<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_cross_track_point_box<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_segment_box<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_segment_box<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

template <typename FP, typename S, typename CT>
struct strategy_converter<strategy::distance::geographic_cross_track_box_box<FP, S, CT> >
{
    static auto get(strategy::distance::geographic_cross_track_box_box<FP, S, CT> const& s)
    {
        return strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>(s.model());
    }
};

// details

// TODO: This specialization wouldn't be needed if strategy::distance::geographic_cross_track was implemented as an alias
template <typename FP, typename S, typename CT, bool B, bool ECP>
struct strategy_converter<strategy::distance::detail::geographic_cross_track<FP, S, CT, B, ECP> >
{
    struct altered_strategy
        : strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT>
    {
        typedef strategies::distance::geographic<FP, strategy::default_order<FP>::value, S, CT> base_t;

        explicit altered_strategy(S const& s) : base_t(s) {}

        using base_t::distance;

        template <typename Geometry1, typename Geometry2>
        auto distance(Geometry1 const&, Geometry2 const&,
                      std::enable_if_t
                      <
                            util::is_pointlike<Geometry1>::value
                                && util::is_segmental<Geometry2>::value
                         || util::is_segmental<Geometry1>::value
                                && util::is_pointlike<Geometry2>::value
                         || util::is_segmental<Geometry1>::value
                                && util::is_segmental<Geometry2>::value
                      > * = nullptr) const
        {
            return strategy::distance::detail::geographic_cross_track
                <
                    FP, S, CT, B, ECP
                >(base_t::m_spheroid);
        }
    };

    static auto get(strategy::distance::detail::geographic_cross_track<FP, S, CT, B, ECP> const& s)
    {
        return altered_strategy(s.model());
    }
};

} // namespace services

}} // namespace strategies::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_DISTANCE_GEOGRAPHIC_HPP


#include "../checkpoint/checkpoint.h"
#include "goexit/goexit.h"
#ifdef HAVE_SSE2
	#include "myrandom/myrandsfmt.h"
#else
	#include "myrandom/myrand.h"
#endif
#include <array>                       	// for std::array
#include <cstdint>  	               	// for std::uint32_t
#include <functional>                   // for std::hash
#include <iomanip>		               	// for std::setiosflags, std::setprecision
#include <iostream> 	               	// for std::cout
#include <string>                      	// for std::string
#include <utility>                      // for std::move
#ifdef _CHECK_PARALELL_PERFORM
    #include <vector>   	            // for std::vector
#endif
#include <boost/container/flat_map.hpp>	// for boost::container::flat_map
#include <tbb/concurrent_hash_map.h>    // for tbb::concurrent_hash_map
#include <tbb/concurrent_vector.h>     	// for tbb::concurrent_vector
#include <tbb/parallel_for.h>           // for tbb::parallel_for

namespace {
    //! A global variable (constant expression).
    /*!
        モンテカルロシミュレーションの試行回数
    */
    static auto constexpr MCMAX = 1000000U;

    //! A global variable (constant expression).
    /*!
        UかDの文字列の長さ
    */
    static auto constexpr RANDNUMTABLELEN = 100U;

    //! A global variable (constant).
    /*!
        UとDの文字列の可能な集合の配列
    */
    static std::array<std::string, 8U> const udarray = { "DDD", "DDU", "DUD", "DUU", "UDD", "UDU", "UUD", "UUU" };

    //! A typedef.
    /*!
        文字列とその文字列に対応する出現回数のstd::map
    */
    using mymap = boost::container::flat_map<std::string, std::uint32_t>;

    //! A typedef.
    /*!
        文字列のペア
    */
    using strpair = std::pair<std::string, std::string>;

    //! A struct.
    /*!
        strpairを扱うハッシュと比較操作を定義する構造体
    */
    struct MyHashCompare final {
        //! A public static member function.
        /*!
            strpairからハッシュを生成する
            \param sp 文字列のペア
            \return 与えられたstrpairのハッシュ
        */
        static std::size_t hash(strpair const & sp)
        {
            auto const h1 = std::hash<std::string>()(sp.first);
            auto const h2 = std::hash<std::string>()(sp.second);
            return h1 ^ (h2 << 1);
        }

        //! A public static member function.
        /*!
            二つのstrpairを比較する
            \param lhs 左辺のstrpair
            \param rhs 右辺のstrpair
            \return 文字列のペアが等しい場合にはtrue
        */
        static bool equal(strpair const & lhs, strpair const & rhs)
        {
            return lhs == rhs;
        }
    };

    //! A typedef.
    /*!
        各文字列の順列に対応する勝利回数の結果を格納するtbb::concurrent_hash_map
    */
    using myhashmap = tbb::concurrent_hash_map<strpair, std::uint32_t, MyHashCompare>;

    //! A typedef.
    /*!
        文字列のペアと、どちらの文字列が勝ったかのstd::pair
    */
    using mymap2 = boost::container::flat_map<strpair, bool>;

    //! A typedef.
    /*!
        文字列のペアと、文字列の勝利数のstd::pair
    */
    using mymap3 = boost::container::flat_map<strpair, std::uint32_t>;

    //! A function.
    /*!
        文字列の可能な順列を列挙する
        \return 文字列の可能な順列を列挙したstd::array
    */
    std::array<strpair, 56U> makecombination();

    //! A global variable (constant).
    /*!
        udarrayから二つを抽出したときの可能な順列の配列
    */
    static std::array<strpair, 56U> const cbarray = makecombination();

#ifdef _CHECK_PARALELL_PERFORM
    //! A function.
    /*!
        文字列のペアの、前者が勝利した回数を集計する
        \param mcresultwinningavg 文字列のペアと、どちらの文字列が勝ったかの結果が格納された連想配列の可変長配列
        \return 文字列のペアの、前者が勝利した回数が格納された連想配列
    */
    mymap3 aggregateWinningAvg(std::vector<mymap2> const & mcresultwinningavg);
#endif

    //! A function.
    /*!
        文字列のペアの、前者が勝利した回数を集計する
        \param mcresultwinningavg 文字列のペアと、どちらの文字列が勝ったかの結果が格納された連想配列の可変長配列
        \return 文字列のペアの、前者が勝利した回数が格納された連想配列
    */
    mymap3 aggregateWinningAvg(tbb::concurrent_vector<mymap2> const & mcresultwinningavg);

    template <typename T>
    //! A template function.
    /*!
        UDのランダム文字列を生成する
        \param mr 自作乱数クラスのオブジェクト
        \return UDのランダム文字列を格納したstd::string
    */
    inline auto makerandomudstr(T & mr);

#ifdef _CHECK_PARALELL_PERFORM
    //! A function.
    /*!
        モンテカルロ・シミュレーションを行う
        \return 期待値と、どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果のstd::pair
    */
    std::pair<std::vector<mymap>, std::vector<mymap2> > montecarlo();
#endif

    //! A function.
    /*!
        モンテカルロ・シミュレーションをTBBで並列化して行う
        \return 期待値と、どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果のstd::pair
    */
    std::pair< tbb::concurrent_vector<mymap>, tbb::concurrent_vector<mymap2> > montecarloTBB();

    template <typename T>
    //! A template function.
    /*!
        期待値に対するモンテカルロ・シミュレーションの実装
        \param mr 自作乱数クラスのオブジェクト
        \return 期待値に対するモンテカルロ・シミュレーションの結果が格納された連想配列
    */
    mymap montecarloImplAvg(T & mr);

    template <typename T>
    //! A template function.
    /*!
        文字列のペアのうち、どちらの文字列が先に出現したかのモンテカルロ・シミュレーションの実装
        \param mr 自作乱数クラスのオブジェクト
        \return 文字列のペアのうち、どちらの文字列が先に出現したかのモンテカルロ・シミュレーションの結果が格納された連想配列
    */
    mymap2 montecarloImplWinningAvg(T & mr);

    //! A function.
    /*!
        UとDのランダム文字列から与えられた文字列の位置を検索し、文字列の末尾の位置を与える
        \param str 検索する文字列
        \param udstr UとDのランダム文字列
        \return 検索された文字列の末尾の位置
    */
    inline std::uint32_t myfind(std::string const & str, std::string const & udstr);

    //! A template function.
    /*!
        期待値に対するモンテカルロ・シミュレーションの和を計算する
        \return 期待値に対するモンテカルロ・シミュレーションの結果の和の連想配列
    */
    template <typename T>
    mymap sumMontecarloAvg(T const & mcresultavg);
}

int main()
{
    checkpoint::CheckPoint cp;

    cp.checkpoint("処理開始", __LINE__);

#ifdef _CHECK_PARALELL_PERFORM
    {
        // モンテカルロ・シミュレーションの結果を代入
        auto const mcresult(montecarlo());

        // 各文字列のペアに対する勝率を計算する
        auto const trialwinningavg(aggregateWinningAvg(mcresult.second));
    }

    cp.checkpoint("並列化無効", __LINE__);
#endif

    // モンテカルロ・シミュレーションの結果を代入
    auto const mcresultTBB(montecarloTBB());

    // 各文字列のペアに対する勝率を計算する
    auto const trialwinningavg(aggregateWinningAvg(mcresultTBB.second));

    cp.checkpoint("並列化有効", __LINE__);

    // 期待値に対するモンテカルロ・シミュレーションの結果の和を計算する
    auto const trialavg(sumMontecarloAvg(mcresultTBB.first));

    // 各文字列に対する期待値の表示
    std::cout << std::setprecision(1) << std::setiosflags(std::ios::fixed);
    for (auto && itr = trialavg.begin(); itr != trialavg.end(); ++itr) {
        std::cout << itr->first
                  << " が出るまでの期待値: "
                  << static_cast<double>(itr->second) / static_cast<double>(MCMAX)
                  << "回\n";
    }

    // 各文字列のペアに対する勝率の表示
    std::cout << "\n    ";
    for (auto i = 0; i < 8; i++) {
        std::cout << udarray[i] << "  ";
    }
    std::cout << '\n';

    auto && itr = trialwinningavg.begin();
    auto const len = udarray.size();
    for (auto i = 0U; i < len; i++) {
        std::cout << itr->first.first << ' ';
        for (auto j = 0U; j < len; j++) {
            if (i == j) {
                std::cout << "     ";
            }
            else {
                std::cout << static_cast<double>(itr->second) / static_cast<double>(MCMAX) * 100.0
                          << ' ';
                ++itr;
            }
        }
        std::cout << '\n';
    }

    cp.checkpoint("それ以外の処理", __LINE__);

    cp.checkpoint_print();

	goexit::goexit();

    return 0;
}

namespace {
#ifdef _CHECK_PARALELL_PERFORM
    mymap3 aggregateWinningAvg(std::vector<mymap2> const & mcresultwinningavg)
    {
        // 各文字列の順列に対応する勝利回数の結果を格納するboost::container::flat_map
        mymap3 trialwinningavg;

        // tbb::concurrent_hash_mapの初期化
        for (auto const & sp : cbarray) {
            trialwinningavg[sp] = 0U;
        }

        // 試行回数分繰り返す
        for (auto const & mcr : mcresultwinningavg) {
            for (auto && itr = mcr.begin(); itr != mcr.end(); ++itr) {
                if (itr->second) {
                    trialwinningavg[itr->first]++;
                }
            }
        }

        return trialwinningavg;
    }
#endif

    mymap3 aggregateWinningAvg(tbb::concurrent_vector<mymap2> const & mcresultwinningavg)
    {
        // 各文字列の順列に対応する勝利回数の結果を格納するtbb::concurrent_hash_map
        myhashmap trial;

        // tbb::concurrent_hash_mapの初期化
        for (auto const & sp : cbarray) {
            myhashmap::accessor a;
            trial.insert(a, sp);
            a->second = 0U;
        }

        // MCMAX回のループを並列化して実行
        tbb::parallel_for(
            tbb::blocked_range<std::uint32_t>(0U, MCMAX),
            [&mcresultwinningavg, &trial](auto const & range) {
            for (auto && i = range.begin(); i != range.end(); ++i) {
                auto const mcr = mcresultwinningavg[i];
                for (auto && itr = mcr.begin(); itr != mcr.end(); ++itr) {
                    if (itr->second) {
                        myhashmap::accessor a;
                        trial.insert(a, itr->first);
                        a->second++;
                    }
                }
            }
        });

        // boost::container::flat_mapに計算結果を複写
        boost::container::flat_map<strpair, std::uint32_t> trialwinningavg;
        for (auto && res : trial) {
            trialwinningavg[res.first] = res.second;
        }

        return trialwinningavg;
    }

    std::array<strpair, 56U> makecombination()
    {
        // 全ての可能な順列を収納する配列
        std::array<strpair, 56U> cb;

        // カウンタ
        auto cnt = 0;

        // 全ての可能な順列を列挙
        auto const len = udarray.size();
        for (auto i = 0U; i < len; i++) {
            for (auto j = 0U; j < len; j++) {
                if (i != j) {
                    cb[cnt++] = std::make_pair(udarray[i], udarray[j]);
                }
            }
        }

        return cb;
    }

    template <typename T>
    auto makerandomudstr(T & mr)
    {
        // UDのランダム文字列を格納するstd::string
        std::string udstring(RANDNUMTABLELEN, '\0');

        // UDのランダム文字列を格納
        for (auto && c : udstring) {
            c = mr.myrand() > 3 ? 'U' : 'D';
        }

		// UDのランダム文字列を返す
        return udstring;
    }

#ifdef _CHECK_PARALELL_PERFORM
    std::pair<std::vector<mymap>, std::vector<mymap2> > montecarlo()
    {
        // 期待値に対するモンテカルロ・シミュレーションの結果を格納するための可変長配列
        std::vector<mymap> mcresultavg;

        // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を格納するための可変長配列
        std::vector<mymap2> mcresultwinningavg;

        // MCMAX個の容量を確保
        mcresultavg.reserve(MCMAX);
        mcresultwinningavg.reserve(MCMAX);

#ifdef HAVE_SSE2
		// 自作乱数クラスを初期化
		myrandom::MyRandSfmt mr(1, 6);
#else
		// 自作乱数クラスを初期化
		myrandom::MyRand mr(1, 6);
#endif

        // 試行回数分繰り返す
        for (auto i = 0U; i < MCMAX; i++) {
            // 期待値に対するモンテカルロ・シミュレーションの結果を代入
            mcresultavg.emplace_back(montecarloImplAvg(mr));

            // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を代入
            mcresultwinningavg.emplace_back(montecarloImplWinningAvg(mr));
        }

        return std::make_pair(std::move(mcresultavg), std::move(mcresultwinningavg));
    }
#endif

    std::pair<tbb::concurrent_vector<mymap>, tbb::concurrent_vector<mymap2> > montecarloTBB()
    {
        // 期待値に対するモンテカルロ・シミュレーションの結果を格納するための可変長配列
        tbb::concurrent_vector<mymap> mcresultavg;

        // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を格納するための可変長配列
        tbb::concurrent_vector<mymap2> mcresultwinningavg;

        // MCMAX個の容量を確保
        mcresultavg.reserve(MCMAX);
        mcresultwinningavg.reserve(MCMAX);

        // MCMAX回のループを並列化して実行
        tbb::parallel_for(
            0U,
            MCMAX,
            1U,
            [&](auto) {

#ifdef HAVE_SSE2
		        // 自作乱数クラスを初期化
		        myrandom::MyRandSfmt mr(1, 6);
#else
		        // 自作乱数クラスを初期化
		        myrandom::MyRand mr(1, 6);
#endif

                // 期待値に対するモンテカルロ・シミュレーションの結果を代入
                mcresultavg.emplace_back(montecarloImplAvg(mr));

                // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を代入
                mcresultwinningavg.emplace_back(montecarloImplWinningAvg(mr));
        });

        return std::make_pair(std::move(mcresultavg), std::move(mcresultwinningavg));
    }

    template <typename T>
    mymap montecarloImplAvg(T & mr)
    {
        // UDのランダム文字列
        auto const udstr(makerandomudstr(mr));

        // 検索結果のstd::map
        mymap result;

        // 文字列が最初に出現するのは何文字目かを検索し結果を代入
        for (auto const & str : udarray) {
            result.insert(std::make_pair(str, myfind(str, udstr)));
        }

        return result;
    }

    template <typename T>
    mymap2 montecarloImplWinningAvg(T & mr)
    {
        // UDのランダム文字列
        auto const udstr(makerandomudstr(mr));

        // 検索結果のstd::map
        mymap2 result;

        // どちらの文字列が先に出現したかの結果を代入
        for (auto const & sp : cbarray) {
            result.insert(std::make_pair(sp, myfind(sp.first, udstr) < myfind(sp.second, udstr)));
        }

        // 検索結果を返す
        return result;
    }

    std::uint32_t myfind(std::string const & str, std::string const & udstr)
    {
        // 文字列の位置を検索
        auto const pos = udstr.find(str);

        // posをその文字列の末尾の位置に変換
        // もし文字列が見つかっていなかった場合はRANDNUMTABLELENに変換
        return pos != std::string::npos ? static_cast<std::uint32_t>(pos + 3) : RANDNUMTABLELEN;
    }

    template <typename T>
    mymap sumMontecarloAvg(T const & mcresultavg)
    {
        // 各文字列に対して、期待値に対するモンテカルロ・シミュレーションの結果の和を格納するstd::map
        mymap trial;

        // std::mapの初期化
        for (auto const & str : udarray) {
            trial[str] = 0;
        }

        // 試行回数分繰り返す
        for (auto const & mcr : mcresultavg) {
            for (auto && itr = mcr.begin(); itr != mcr.end(); ++itr) {
                trial[itr->first] += itr->second;
            }
        }

        return trial;
    }
}


#include <stdio.h>
#include <string.h>
#include <iostream>
#include <iterator>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <urdf/model.h>
#include <kdl_parser/kdl_parser.hpp>
#include "ros/ros.h"

#include "snoptProblem.hpp"

using namespace std;
using namespace Eigen;

const string prefix     = "j2s7s300";
const double pi         = 3.1415926535897;

const int T             = 10.0;         // planning horizon
const double dt         = 0.5;          // timestep
const int dofRobot      = 3;            // num dofs of robot
const int dofEnv        = 1;            // num dofs of environment
const int dofLamb       =  1;           // dimension of contact forces

const double goal[] = {-0.4, 0.5};
const double obsLow[]   = {-1.0, 0.3};
const double obsHigh[]  = {1.0, 0.3};

// --------- HELPER FUNCTIONS --------- //

VectorXf config2DTo3D(VectorXf& q2D){
  VectorXf q3D(10);
  q3D << q2D(0), pi/2.0, pi/2.0, q2D(1), pi, q2D(2), pi, 0, 0, 0;
  return q3D;
}

VectorXf config3DTo2D(VectorXf& q3D){
  VectorXf q2D(3);
  q2D << q3D(0), q3D(3), q3D(5);
  return q2D;
}

VectorXf vel2DTo3D(VectorXf& dq2D){
  VectorXf dq3D(10);
  dq3D << dq2D(0), 0, 0, dq2D(1), 0, dq2D(2), 0, 0, 0, 0;
  return dq3D;
}

VectorXf vel3DTo2D(VectorXf& dq3D){
  VectorXf dq2D(3);
  dq2D << dq3D(0), dq3D(3), dq3D(5);
  return dq2D;
}

VectorXf control2DTo3D(VectorXf& u2D){
  VectorXf u3D(10);
  u3D << u2D(0), 0, 0, u2D(1), 0, u2D(2), 0, 0, 0, 0;
  return u3D;
}

VectorXf control3DTo2D(VectorXf& u3D){
  VectorXf u2D(3);
  u2D << u3D(0), u3D(3), u3D(5);
  return u2D;
}

VectorXf acc2DTo3D(VectorXf& ddq2D){
  VectorXf ddq3D(10);
  ddq3D << ddq2D(0), 0, 0, ddq2D(1), 0, ddq2D(2), 0, 0, 0, 0;
  return ddq3D;
}

VectorXf acc3DTo2D(VectorXf& ddq3D){
  VectorXf ddq2D(3);
  ddq2D << ddq3D(0), ddq3D(3), ddq3D(5);
  return ddq2D;
}

// --------- GETTER FUNCTIONS --------- //

double getFinalCost(VectorXf& q, VectorXf& dq){
  VectorXf q3D = config2DTo3D(q);
  //string linkName = strcat(prefix, "_link_7");
  //vector<double> eePose = getTransform(robot, q3d, linkName);
  // VectorXf goal(2);
  // goalPose << goal[0], goal[1];
  //double gf = 30*(goalPose - eePose).norm();
  double gf = 0.0;
  return gf;
}

double getRunningCost(VectorXf& q, VectorXf& dq, VectorXf& u){
  return 0.0;
}

VectorXf getConfigRobot(double *x, int t){
  VectorXf qRobot(dofRobot);
  int lowOffset = 2*(dofRobot + dofEnv)*t + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    qRobot(ii-lowOffset) = x[ii];
  return qRobot;
}

VectorXf getVelRobot(double *x, int t){
  VectorXf dqRobot(dofRobot);
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    dqRobot(ii-lowOffset) = x[ii];
  return dqRobot;
}

VectorXf getConfigEnv(double *x, int t){
  VectorXf qEnv(dofEnv); //= new double[dofEnv];
  int lowOffset = 2*(dofRobot + dofEnv)*t + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot + dofEnv;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    qEnv(ii-lowOffset) = x[ii];
  return qEnv;
}

VectorXf getVelEnv(double *x, int t){
  VectorXf dqEnv(dofEnv);
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + 2*(dofRobot + dofEnv);
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    dqEnv(ii-lowOffset) = x[ii];
  return dqEnv;
}

VectorXf getU(double *x, int t){
  VectorXf u(dofRobot);
  int nx = 2*(dofRobot+dofEnv)*(T+1);
  int lowOffset = nx + 1 + dofRobot*(t - 1);
  int upOffset  = nx + 1 + dofRobot*t - 1;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    u(ii-lowOffset) = x[ii];
  return u;
}
// ------------------------------------ //

// --------- SETTER FUNCTIONS --------- //

void setConfigRobot(double *x, int t, double *q){
  int lowOffset = 2*(dofRobot + dofEnv)*t + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot;
  copy(x + lowOffset, x + upOffset, q);
}

void setConfigEnv(double *x, int t, double *q){
  int lowOffset = 2*(dofRobot + dofEnv)*t + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot + dofEnv;
  copy(x + lowOffset, x + upOffset, q);
}

void setVelRobot(double *x, int t, double *dq){
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot;
  copy(x + lowOffset, x + upOffset, dq);
}

void setVelEnv(double *x, int t, double *dq){
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + 2*(dofRobot + dofEnv);
  copy(x + lowOffset, x + upOffset, dq);
}

void setU(double *x, int t, double *u, int nx){
  int uStart    = nx + 1;
  int lowOffset = uStart + dofRobot*(t-1);
  int upOffset  = uStart + dofRobot*t - 1;
  copy(x + lowOffset, x + upOffset, u);
}

void setLambda(double *x, int t, double *lambda, int nx, int nu){
  int lambStart = nx + nu + 1;
  int lowOffset = lambStart + dofLamb*(t-1);
  int upOffset  = lambStart + dofLamb*t - 1;
  copy(x + lowOffset, x + upOffset, lambda);
}

// ------------------------------------ //

void userfun(int    *Status, int *n,    double x[],
       int    *needF,  int *neF,  double F[],
       int    *needG,  int *neG,  double G[],
       char      *cu,  int *lencu,
       int    iu[],    int *leniu,
       double ru[],    int *lenru) {

  // TODO IMPLEMENT ME!
  VectorXf qRobot_T   = getConfigRobot(x, T);
  VectorXf dqRobot_T  = getVelRobot(x, T);
  double obj          = getFinalCost(qRobot_T, dqRobot_T);

  int kinLen  = (dofRobot + dofEnv)*T;
  int dynLen  = (dofRobot + dofEnv)*T;
  int collLen = T;

  // Compute kinematic, dynamics, and collision constraints
  double *kinConstraints = new double[kinLen];
  double *dynConstraints = new double[dynLen];
  double *collConstraints = new double[collLen];

  for(int t=0; t<T-1; t++){
    VectorXf qRobot_t  = getConfigRobot(x, t);
    VectorXf qRobot_t1 = getConfigRobot(x, t+1);

    VectorXf dqRobot_t   = getVelRobot(x, t);
    VectorXf dqRobot_t1  = getVelRobot(x, t+1);

    VectorXf dqEnv_t   = getVelEnv(x, t);
    VectorXf dqEnv_t1  = getVelEnv(x, t+1);

    VectorXf qEnv_t  = getConfigEnv(x, t);
    VectorXf qEnv_t1 = getConfigEnv(x, t+1);

    VectorXf ddqRobot_t1  = (dqRobot_t1 - dqRobot_t)/dt;
    VectorXf ddqEnv_t1    = (dqEnv_t1 - dqEnv_t)/dt;

    VectorXf qRobot3D_t1  = config2DTo3D(qRobot_t1);
    VectorXf dqRobot3D_t1 = vel2DTo3D(qRobot_t1);

    VectorXf u_t1   = getU(x, t+1);
    VectorXf u3D_t1 = control2DTo3D(u_t1);

    //VectorXf ddqRobot3D_t1_fwd  = forwardDynamics(robot, qRobot3D_t1, dqRobot3D_t1, u3D_t1);
    //VectorXf ddqRobot_t1_fwd    = acc3DTo2D(ddqRobot3D_t1_fwd);

    obj += dt*getRunningCost(qRobot_t, dqRobot_t, u_t1);
  }

  int kinStart  = 1;
  int dynStart  = kinStart + kinLen + 1;
  int collStart = dynStart + dynLen + 1;

  // Set up the constraint vector
  F[0] = obj;
  copy(F + kinStart, F + kinStart + kinLen, kinConstraints);
  copy(F + dynStart, F + dynStart + dynLen, dynConstraints);
  copy(F + collStart, F + collStart + collLen, collConstraints);
}

int main(int argc, char **argv) {
  snoptProblemA ToyProb;

  string urdf_file = "../urdf/jaco_dynamics.urdf";

  /*urdf::Model robot;
  if (!robot.initString(urdf_file)) {
    ROS_FATAL("Could not initialize robot model");
    return -1;
  }

  KDL::Tree tree;
  if (!kdl_parser::treeFromFile(urdf_file, tree)){
    ROS_ERROR("Failed to construct kdl tree");
    return false;
  }*/

  // Allocate and initialize;
  int nx        =  2*(dofRobot+dofEnv)*(T+1); // num states over time horizon
  int nu        =  dofRobot*T;        // num controls over time horizon
  int nlamb     =  T;

  int neF       =  1 + 2*(dofRobot+dofEnv)*T+T; // num constraints
  int n         =  nx + nu + nlamb;

  int nS = 0, nInf;
  double sInf;

  double *x      = new double[n];
  double *xlow   = new double[n];
  double *xupp   = new double[n];
  double *xmul   = new double[n];
  int    *xstate = new    int[n];

  double *F      = new double[neF];
  double *Flow   = new double[neF];
  double *Fupp   = new double[neF];
  double *Fmul   = new double[neF];
  int    *Fstate = new int[neF];    // vector of inittial states of F(x)

  int    ObjRow  = 0;   // constant value added to the objective
  double ObjAdd  = 0;   // row of F(x) containing the objective

  int Cold = 0, Basis = 1, Warm = 2;
  int signedDistLinkStart = 1 + 2*(dofRobot + dofEnv)*T;

  double *qRobotInit    = new double[dofRobot];
  double *qRobotMin     = new double[dofRobot]; // all zeros
  double *qRobotMax     = new double[dofRobot];

  double *qEnvInit      = new double[dofEnv];   // all zeros
  double *qEnvMin       = new double[dofEnv];   // all zeros
  double *qEnvMax       = new double[dofEnv];

  double *dqRobotInit   = new double[dofRobot]; // all zeros
  double *dqRobotMin    = new double[dofRobot]; // all zeros
  double *dqRobotMax    = new double[dofRobot];

  double *dqEnvInit     = new double[dofEnv];   // all zeros
  double *dqEnvMin      = new double[dofEnv];   // all zeros
  double *dqEnvMax      = new double[dofEnv];

  double *lambMin       = new double[dofLamb];  // all zeros
  double *lambMax       = new double[dofLamb];

  double *uMin          = new double[dofRobot];
  double *uMax          = new double[dofRobot];

  // --------- Set up init, max and min  --------- //

  fill(qRobotInit, qRobotInit + dofRobot, pi);
  fill(qRobotMax, qRobotMax + dofRobot, 2.0*pi);

  fill(dqRobotMin, dqRobotMin + dofRobot, -0.35);
  fill(dqRobotMax, dqRobotMax + dofRobot, 0.35);

  fill(qEnvMax, qEnvMax + dofEnv, 100.0);
  fill(dqEnvMax, dqEnvMax + dofEnv, 10.0);

  fill(uMin, uMin + dofRobot, -100.0);
  fill(uMax, uMax + dofRobot, 100.0);

  fill(lambMax, lambMax + dofLamb, 1000.0);

  // --------------------------------------------- //

  // --------- Set up the constraints on x --------- //

  for(int t = 0;t < T;t++){
    if(t == 0){
      // Constrain initial config and velocity
      setConfigRobot(xlow, t, qRobotInit);
      setConfigRobot(xupp, t, qRobotInit);

      setVelRobot(xlow, t, dqRobotInit);
      setVelRobot(xupp, t, dqRobotInit);

      setConfigEnv(xlow, t, qEnvInit);
      setConfigEnv(xupp, t, qEnvInit);

      setVelEnv(xlow, t, dqEnvInit);
      setVelEnv(xupp, t, dqEnvInit);

    }else{
      // Constrain joint limits for rest of trajectory
      setConfigRobot(xlow, t, qRobotMin);
      setConfigRobot(xupp, t, qRobotMax);

      setVelRobot(xlow, t, dqRobotMin);
      setVelRobot(xupp, t, dqRobotMax);

      setConfigEnv(xlow, t, qEnvMin);
      setConfigEnv(xupp, t, qEnvMax);

      setVelEnv(xlow, t, dqEnvMin);
      setVelEnv(xupp, t, dqEnvMax);
    }

    if(t > 0){
      // Set torque limits
      setU(xlow, t, uMin, nx);
      setU(xupp, t, uMax, nx);

      // Set lambda constraint forces
      setLambda(xlow, t, lambMin, nx, nu);
      setLambda(xupp, t, lambMax, nx, nu);
    }
  }

  // ----------------------------------------------- //

  // Set bounds on the objective
  Flow[0] = -1e3;
  Fupp[0] =  1e3;
  fill(Fupp + signedDistLinkStart, Fupp + signedDistLinkStart + T, 1000.0);

  // Load the data for planContact ...
  ToyProb.initialize    ("", 1);          // no print file; summary on
  ToyProb.setPrintFile  ("Contact.out");  // oh wait, i want a print file
  ToyProb.setProbName   ("Contact");

  // snopta will compute the Jacobian by finite-differences.
  // snJac will be called  to define the
  // coordinate arrays (iAfun,jAvar,A) and (iGfun, jGvar).
  ToyProb.setIntParameter("Derivative option", 0);
  ToyProb.setIntParameter("Verify level ", 3);

  // Solve the problem.
  // snJac is called implicitly in this case to compute the Jacobian.
  ToyProb.solve(Cold, neF, n, ObjAdd, ObjRow, userfun,
		xlow, xupp, Flow, Fupp,
		x, xstate, xmul, F, Fstate, Fmul,
		nS, nInf, sInf);

  for (int i = 0; i < n; i++){
    cout << "x = " << x[i] << " xstate = " << xstate[i] << endl;
  }

  delete []x;      delete []xlow;   delete []xupp;
  delete []xmul;   delete []xstate;

  delete []F;      delete []Flow;   delete []Fupp;
  delete []Fmul;   delete []Fstate;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2006 Marco Bianchetti
 Copyright (C) 2007 StatPro Italia srl
 Copyright (C) 2014 Ferdinando Ametrano
 Copyright (C) 2016 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file swaption.hpp
    \brief Swaption class
*/

#ifndef quantlib_instruments_swaption_hpp
#define quantlib_instruments_swaption_hpp

#include <ql/option.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/volatility/volatilitytype.hpp>

namespace QuantLib {

    //! %settlement information
    struct Settlement {
        enum Type { Physical, Cash };
    };

    std::ostream& operator<<(std::ostream& out,
                             Settlement::Type type);

    //! %Swaption class
    /*! \ingroup instruments

        \test
        - the correctness of the returned value is tested by checking
          that the price of a payer (resp. receiver) swaption
          decreases (resp. increases) with the strike.
        - the correctness of the returned value is tested by checking
          that the price of a payer (resp. receiver) swaption
          increases (resp. decreases) with the spread.
        - the correctness of the returned value is tested by checking
          it against that of a swaption on a swap with no spread and a
          correspondingly adjusted fixed rate.
        - the correctness of the returned value is tested by checking
          it against a known good value.
        - the correctness of the returned value of cash settled swaptions
          is tested by checking the modified annuity against a value
          calculated without using the Swaption class.

        \todo add greeks and explicit exercise lag
    */
    class Swaption : public Option {
      public:
        class arguments;
        class engine;
        Swaption(const boost::shared_ptr<VanillaSwap>& swap,
                 const boost::shared_ptr<Exercise>& exercise,
                 Settlement::Type delivery = Settlement::Physical);
        //! \name Instrument interface
        //@{
        bool isExpired() const;
        void setupArguments(PricingEngine::arguments*) const;
        //@}
        //! \name Inspectors
        //@{
        Settlement::Type settlementType() const { return settlementType_; }
        VanillaSwap::Type type() const { return swap_->type(); }
        const boost::shared_ptr<VanillaSwap>& underlyingSwap() const {
            return swap_;
        }
        //@}
        //! implied volatility
        Volatility impliedVolatility(
                              Real price,
                              const Handle<YieldTermStructure>& discountCurve,
                              Volatility guess,
                              Real accuracy = 1.0e-4,
                              Natural maxEvaluations = 100,
                              Volatility minVol = 1.0e-7,
                              Volatility maxVol = 4.0,
                              VolatilityType type = ShiftedLognormal,
                              Real displacement = 0.0) const;
      private:
        // arguments
        boost::shared_ptr<VanillaSwap> swap_;
        //Handle<YieldTermStructure> termStructure_;
        Settlement::Type settlementType_;
    };

    //! %Arguments for swaption calculation
    class Swaption::arguments : public VanillaSwap::arguments,
                                public Option::arguments {
      public:
        arguments() : settlementType(Settlement::Physical) {}
        boost::shared_ptr<VanillaSwap> swap;
        Settlement::Type settlementType;
        void validate() const;
    };

    //! base class for swaption engines
    class Swaption::engine
        : public GenericEngine<Swaption::arguments, Swaption::results> {};

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2006 Marco Bianchetti
 Copyright (C) 2007 StatPro Italia srl
 Copyright (C) 2014 Ferdinando Ametrano
 Copyright (C) 2016 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#include <ql/pricingengines/swaption/blackswaptionengine.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/exercise.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

    namespace {

        class ImpliedSwaptionVolHelper {
          public:
            ImpliedSwaptionVolHelper(const Swaption&,
                                     const Handle<YieldTermStructure>& discountCurve,
                                     Real targetValue,
                                     Real displacement,
                                     VolatilityType type);
            Real operator()(Volatility x) const;
            Real derivative(Volatility x) const;
          private:
            boost::shared_ptr<PricingEngine> engine_;
            Handle<YieldTermStructure> discountCurve_;
            Real targetValue_;
            boost::shared_ptr<SimpleQuote> vol_;
            const Instrument::results* results_;
        };

    inline     ImpliedSwaptionVolHelper::ImpliedSwaptionVolHelper(
                              const Swaption& swaption,
                              const Handle<YieldTermStructure>& discountCurve,
                              Real targetValue,
                              Real displacement,
                              VolatilityType type)
        : discountCurve_(discountCurve), targetValue_(targetValue) {

            // set an implausible value, so that calculation is forced
            // at first ImpliedSwaptionVolHelper::operator()(Volatility x) call
            vol_ = boost::shared_ptr<SimpleQuote>(new SimpleQuote(-1.0));
            Handle<Quote> h(vol_);

            switch (type) {
            case ShiftedLognormal:
                engine_ = boost::make_shared<BlackSwaptionEngine>(
                    discountCurve_, h, Actual365Fixed(), displacement);
                break;
            case Normal:
                engine_ = boost::make_shared<BachelierSwaptionEngine>(
                    discountCurve_, h, Actual365Fixed());
                break;
            default:
                QL_FAIL("unknown VolatilityType (" << type << ")");
                break;
            }
            swaption.setupArguments(engine_->getArguments());
            results_ = dynamic_cast<const Instrument::results *>(
                engine_->getResults());
        }

    inline     Real ImpliedSwaptionVolHelper::operator()(Volatility x) const {
            if (x!=vol_->value()) {
                vol_->setValue(x);
                engine_->calculate();
            }
            return results_->value-targetValue_;
        }

    inline     Real ImpliedSwaptionVolHelper::derivative(Volatility x) const {
            if (x!=vol_->value()) {
                vol_->setValue(x);
                engine_->calculate();
            }
            std::map<std::string,boost::any>::const_iterator vega_ =
                results_->additionalResults.find("vega");
            QL_REQUIRE(vega_ != results_->additionalResults.end(),
                       "vega not provided");
            return boost::any_cast<Real>(vega_->second);
        }
    }

    inline std::ostream& operator<<(std::ostream& out,
                             Settlement::Type t) {
        switch (t) {
          case Settlement::Physical:
            return out << "Delivery";
          case Settlement::Cash:
            return out << "Cash";
          default:
            QL_FAIL("unknown Settlement::Type(" << Integer(t) << ")");
        }
    }

    inline Swaption::Swaption(const boost::shared_ptr<VanillaSwap>& swap,
                       const boost::shared_ptr<Exercise>& exercise,
                       Settlement::Type delivery)
    : Option(boost::shared_ptr<Payoff>(), exercise), swap_(swap),
      settlementType_(delivery) {
        registerWith(swap_);
        registerWithObservables(swap_);
    }

    inline bool Swaption::isExpired() const {
        return detail::simple_event(exercise_->dates().back()).hasOccurred();
    }

    inline void Swaption::setupArguments(PricingEngine::arguments* args) const {

        swap_->setupArguments(args);

        Swaption::arguments* arguments =
            dynamic_cast<Swaption::arguments*>(args);

        QL_REQUIRE(arguments != 0, "wrong argument type");

        arguments->swap = swap_;
        arguments->settlementType = settlementType_;
        arguments->exercise = exercise_;
    }

    inline void Swaption::arguments::validate() const {
        VanillaSwap::arguments::validate();
        QL_REQUIRE(swap, "vanilla swap not set");
        QL_REQUIRE(exercise, "exercise not set");
    }

    inline Volatility Swaption::impliedVolatility(Real targetValue,
                                           const Handle<YieldTermStructure>& d,
                                           Volatility guess,
                                           Real accuracy,
                                           Natural maxEvaluations,
                                           Volatility minVol,
                                           Volatility maxVol,
                                           VolatilityType type,
                                           Real displacement) const {
        //calculate();
        QL_REQUIRE(!isExpired(), "instrument expired");

        ImpliedSwaptionVolHelper f(*this, d, targetValue, displacement, type);
        //Brent solver;
        NewtonSafe solver;
        solver.setMaxEvaluations(maxEvaluations);
        return solver.solve(f, accuracy, guess, minVol, maxVol);
    }

}

#endif


// $Id$
//
//  Copyright (C) 2007-2013 Greg Landrum
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//

#include <GraphMol/RDKitBase.h>
#include <GraphMol/Fingerprints/AtomPairs.h>
#include <GraphMol/Subgraphs/Subgraphs.h>
#include <DataStructs/SparseIntVect.h>
#include <RDGeneral/hash/hash.hpp>
#include <boost/cstdint.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/foreach.hpp>

namespace RDKit {
namespace AtomPairs {
unsigned int numPiElectrons(const Atom *atom) {
  PRECONDITION(atom, "no atom");
  unsigned int res = 0;
  if (atom->getIsAromatic()) {
    res = 1;
  } else if (atom->getHybridization() != Atom::SP3) {
    unsigned int val = static_cast<unsigned int>(atom->getExplicitValence());
    val -= atom->getNumExplicitHs();
    CHECK_INVARIANT(val >= atom->getDegree(),
                    "explicit valence exceeds atom degree");
    res = val - atom->getDegree();
  }
  return res;
}

boost::uint32_t getAtomCode(const Atom *atom, unsigned int branchSubtract,
                            bool includeChirality) {
  PRECONDITION(atom, "no atom");
  boost::uint32_t code;

  unsigned int numBranches = 0;
  if (atom->getDegree() > branchSubtract) {
    numBranches = atom->getDegree() - branchSubtract;
  }

  code = numBranches % maxNumBranches;
  unsigned int nPi = numPiElectrons(atom) % maxNumPi;
  code |= nPi << numBranchBits;

  unsigned int typeIdx = 0;
  unsigned int nTypes = 1 << numTypeBits;
  while (typeIdx < nTypes) {
    if (atomNumberTypes[typeIdx] ==
        static_cast<unsigned int>(atom->getAtomicNum())) {
      break;
    } else if (atomNumberTypes[typeIdx] >
               static_cast<unsigned int>(atom->getAtomicNum())) {
      typeIdx = nTypes;
      break;
    }
    ++typeIdx;
  }
  if (typeIdx == nTypes) --typeIdx;
  code |= typeIdx << (numBranchBits + numPiBits);
  if (includeChirality) {
    std::string cipCode;
    if (atom->getPropIfPresent(common_properties::_CIPCode, cipCode)) {
      boost::uint32_t offset = numBranchBits + numPiBits + numTypeBits;
      if (cipCode == "R") {
        code |= 1 << offset;
      } else if (cipCode == "S") {
        code |= 2 << offset;
      }
    }
  }
  POSTCONDITION(code < static_cast<boost::uint32_t>(1 << (codeSize + (includeChirality ? 2 : 0))),
                "code exceeds number of bits");
  return code;
};

boost::uint32_t getAtomPairCode(boost::uint32_t codeI, boost::uint32_t codeJ,
                                unsigned int dist, bool includeChirality) {
  PRECONDITION(dist < maxPathLen, "dist too long");
  boost::uint32_t res = dist;
  res |= std::min(codeI, codeJ) << numPathBits;
  res |= std::max(codeI, codeJ)
         << (numPathBits + codeSize + (includeChirality ? numChiralBits : 0));
  return res;
}

template <typename T1, typename T2>
void updateElement(SparseIntVect<T1> &v, T2 elem) {
  v.setVal(elem, v.getVal(elem) + 1);
}

template <typename T1>
void updateElement(ExplicitBitVect &v, T1 elem) {
  v.setBit(elem % v.getNumBits());
}

template <typename T>
void setAtomPairBit(boost::uint32_t i, boost::uint32_t j,
                    boost::uint32_t nAtoms,
                    const std::vector<boost::uint32_t> &atomCodes,
                    const double *dm, T *bv, unsigned int minLength,
                    unsigned int maxLength, bool includeChirality) {
  unsigned int dist = static_cast<unsigned int>(floor(dm[i * nAtoms + j]));
  if (dist >= minLength && dist <= maxLength) {
    boost::uint32_t bitId =
        getAtomPairCode(atomCodes[i], atomCodes[j], dist, includeChirality);
    updateElement(*bv, static_cast<boost::uint32_t>(bitId));
  }
}

SparseIntVect<boost::int32_t> *getAtomPairFingerprint(
    const ROMol &mol, const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants, bool includeChirality,
    bool use2D, int confId) {
  return getAtomPairFingerprint(mol, 1, maxPathLen - 1, fromAtoms, ignoreAtoms,
                                atomInvariants, includeChirality, use2D,
                                confId);
};

SparseIntVect<boost::int32_t> *getAtomPairFingerprint(
    const ROMol &mol, unsigned int minLength, unsigned int maxLength,
    const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants, bool includeChirality,
    bool use2D, int confId) {
  PRECONDITION(minLength <= maxLength, "bad lengths provided");
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  SparseIntVect<boost::int32_t> *res = new SparseIntVect<boost::int32_t>(
      1 << (numAtomPairFingerprintBits + 2 * (includeChirality ? 2 : 0)));
  const double *dm;
  if (use2D) {
    dm = MolOps::getDistanceMat(mol);
  } else {
    dm = MolOps::get3DDistanceMat(mol, confId);
  }
  const unsigned int nAtoms = mol.getNumAtoms();

  std::vector<boost::uint32_t> atomCodes;
  for (ROMol::ConstAtomIterator atomItI = mol.beginAtoms();
       atomItI != mol.endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      atomCodes.push_back((*atomInvariants)[(*atomItI)->getIdx()] %
                          ((1 << codeSize) - 1));
    }
  }

  for (ROMol::ConstAtomIterator atomItI = mol.beginAtoms();
       atomItI != mol.endAtoms(); ++atomItI) {
    unsigned int i = (*atomItI)->getIdx();
    if (ignoreAtoms &&
        std::find(ignoreAtoms->begin(), ignoreAtoms->end(), i) !=
            ignoreAtoms->end()) {
      continue;
    }
    if (!fromAtoms) {
      for (ROMol::ConstAtomIterator atomItJ = atomItI + 1;
           atomItJ != mol.endAtoms(); ++atomItJ) {
        unsigned int j = (*atomItJ)->getIdx();
        if (ignoreAtoms &&
            std::find(ignoreAtoms->begin(), ignoreAtoms->end(), j) !=
                ignoreAtoms->end()) {
          continue;
        }
        setAtomPairBit(i, j, nAtoms, atomCodes, dm, res, minLength, maxLength,
                       includeChirality);
      }
    } else {
      BOOST_FOREACH (boost::uint32_t j, *fromAtoms) {
        if (j != i) {
          if (ignoreAtoms &&
              std::find(ignoreAtoms->begin(), ignoreAtoms->end(), j) !=
                  ignoreAtoms->end()) {
            continue;
          }
          setAtomPairBit(i, j, nAtoms, atomCodes, dm, res, minLength, maxLength,
                         includeChirality);
        }
      }
    }
  }
  return res;
}

SparseIntVect<boost::int32_t> *getHashedAtomPairFingerprint(
    const ROMol &mol, unsigned int nBits, unsigned int minLength,
    unsigned int maxLength, const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants, bool includeChirality,
    bool use2D, int confId) {
  PRECONDITION(minLength <= maxLength, "bad lengths provided");
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  SparseIntVect<boost::int32_t> *res = new SparseIntVect<boost::int32_t>(nBits);
  const double *dm;
  if (use2D) {
    dm = MolOps::getDistanceMat(mol);
  } else {
    dm = MolOps::get3DDistanceMat(mol, confId);
  }

  const unsigned int nAtoms = mol.getNumAtoms();

  std::vector<boost::uint32_t> atomCodes;
  atomCodes.reserve(nAtoms);
  for (ROMol::ConstAtomIterator atomItI = mol.beginAtoms();
       atomItI != mol.endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      atomCodes.push_back((*atomInvariants)[(*atomItI)->getIdx()]);
    }
  }

  for (ROMol::ConstAtomIterator atomItI = mol.beginAtoms();
       atomItI != mol.endAtoms(); ++atomItI) {
    unsigned int i = (*atomItI)->getIdx();
    if (ignoreAtoms &&
        std::find(ignoreAtoms->begin(), ignoreAtoms->end(), i) !=
            ignoreAtoms->end()) {
      continue;
    }
    if (!fromAtoms) {
      for (ROMol::ConstAtomIterator atomItJ = atomItI + 1;
           atomItJ != mol.endAtoms(); ++atomItJ) {
        unsigned int j = (*atomItJ)->getIdx();
        if (ignoreAtoms &&
            std::find(ignoreAtoms->begin(), ignoreAtoms->end(), j) !=
                ignoreAtoms->end()) {
          continue;
        }
        unsigned int dist =
            static_cast<unsigned int>(floor(dm[i * nAtoms + j]));
        if (dist >= minLength && dist <= maxLength) {
          boost::uint32_t bit = 0;
          gboost::hash_combine(bit, std::min(atomCodes[i], atomCodes[j]));
          gboost::hash_combine(bit, dist);
          gboost::hash_combine(bit, std::max(atomCodes[i], atomCodes[j]));
          updateElement(*res, static_cast<boost::int32_t>(bit % nBits));
        }
      }
    } else {
      BOOST_FOREACH (boost::uint32_t j, *fromAtoms) {
        if (j != i) {
          if (ignoreAtoms &&
              std::find(ignoreAtoms->begin(), ignoreAtoms->end(), j) !=
                  ignoreAtoms->end()) {
            continue;
          }
          unsigned int dist =
              static_cast<unsigned int>(floor(dm[i * nAtoms + j]));
          if (dist >= minLength && dist <= maxLength) {
            boost::uint32_t bit = 0;
            gboost::hash_combine(bit, std::min(atomCodes[i], atomCodes[j]));
            gboost::hash_combine(bit, dist);
            gboost::hash_combine(bit, std::max(atomCodes[i], atomCodes[j]));
            updateElement(*res, static_cast<boost::int32_t>(bit % nBits));
          }
        }
      }
    }
  }
  return res;
}

ExplicitBitVect *getHashedAtomPairFingerprintAsBitVect(
    const ROMol &mol, unsigned int nBits, unsigned int minLength,
    unsigned int maxLength, const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants,
    unsigned int nBitsPerEntry, bool includeChirality, bool use2D, int confId) {
  PRECONDITION(minLength <= maxLength, "bad lengths provided");
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  static int bounds[4] = {1, 2, 4, 8};

  unsigned int blockLength = nBits / nBitsPerEntry;
  SparseIntVect<boost::int32_t> *sres = getHashedAtomPairFingerprint(
      mol, blockLength, minLength, maxLength, fromAtoms, ignoreAtoms,
      atomInvariants, includeChirality, use2D, confId);
  ExplicitBitVect *res = new ExplicitBitVect(nBits);
  if (nBitsPerEntry != 4) {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second > static_cast<int>(i))
          res->setBit(val.first * nBitsPerEntry + i);
      }
    }
  } else {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second >= bounds[i]) {
          res->setBit(val.first * nBitsPerEntry + i);
        }
      }
    }
  }
  delete sres;
  return res;
}

boost::uint64_t getTopologicalTorsionCode(
    const std::vector<boost::uint32_t> &pathCodes, bool includeChirality) {
  bool reverseIt = false;
  unsigned int i = 0;
  unsigned int j = pathCodes.size() - 1;
  while (i < j) {
    if (pathCodes[i] > pathCodes[j]) {
      reverseIt = true;
      break;
    } else if (pathCodes[i] < pathCodes[j]) {
      break;
    }
    ++i;
    --j;
  }

  int shiftSize = codeSize + (includeChirality ? numChiralBits : 0);
  boost::uint64_t res = 0;
  if (reverseIt) {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      res |= static_cast<boost::uint64_t>(pathCodes[pathCodes.size() - i - 1])
             << (shiftSize * i);
    }
  } else {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      res |= static_cast<boost::uint64_t>(pathCodes[i]) << (shiftSize * i);
    }
  }
  return res;
}

size_t getTopologicalTorsionHash(
    const std::vector<boost::uint32_t> &pathCodes) {
  bool reverseIt = false;
  unsigned int i = 0;
  unsigned int j = pathCodes.size() - 1;
  while (i < j) {
    if (pathCodes[i] > pathCodes[j]) {
      reverseIt = true;
      break;
    } else if (pathCodes[i] < pathCodes[j]) {
      break;
    }
    ++i;
    --j;
  }

  boost::uint32_t res = 0;
  if (reverseIt) {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      gboost::hash_combine(res, pathCodes[pathCodes.size() - i - 1]);
    }
  } else {
    for (unsigned int i = 0; i < pathCodes.size(); ++i) {
      gboost::hash_combine(res, pathCodes[i]);
    }
  }
  return res;
}

SparseIntVect<boost::int64_t> *getTopologicalTorsionFingerprint(
    const ROMol &mol, unsigned int targetSize,
    const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants, bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  boost::uint64_t sz = 1;
  sz = (sz << (targetSize *
               (codeSize + (includeChirality ? numChiralBits : 0))));
  // NOTE: this -1 is incorrect but it's needed for backwards compatibility.
  //  hopefully we'll never have a case with a torsion that hits this.
  //
  //  mmm, bug compatible.
  sz -= 1;
  SparseIntVect<boost::int64_t> *res = new SparseIntVect<boost::int64_t>(sz);

  std::vector<boost::uint32_t> atomCodes;
  atomCodes.reserve(mol.getNumAtoms());
  for (ROMol::ConstAtomIterator atomItI = mol.beginAtoms();
       atomItI != mol.endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      // need to add to the atomCode here because we subtract off up to 2 below
      // as part of the branch correction
      atomCodes.push_back(
          (*atomInvariants)[(*atomItI)->getIdx()] % ((1 << codeSize) - 1) + 2);
    }
  }

  boost::dynamic_bitset<> *fromAtomsBV = 0;
  if (fromAtoms) {
    fromAtomsBV = new boost::dynamic_bitset<>(mol.getNumAtoms());
    BOOST_FOREACH (boost::uint32_t fAt, *fromAtoms) { fromAtomsBV->set(fAt); }
  }
  boost::dynamic_bitset<> *ignoreAtomsBV = 0;
  if (ignoreAtoms) {
    ignoreAtomsBV = new boost::dynamic_bitset<>(mol.getNumAtoms());
    BOOST_FOREACH (boost::uint32_t fAt, *ignoreAtoms) {
      ignoreAtomsBV->set(fAt);
    }
  }
  boost::dynamic_bitset<> pAtoms(mol.getNumAtoms());
  PATH_LIST paths = findAllPathsOfLengthN(mol, targetSize, false);
  for (PATH_LIST::const_iterator pathIt = paths.begin(); pathIt != paths.end();
       ++pathIt) {
    bool keepIt = true;
    if (fromAtomsBV) {
      keepIt = false;
    }
    std::vector<boost::uint32_t> pathCodes;
    const PATH_TYPE &path = *pathIt;
    if (fromAtomsBV) {
      if (fromAtomsBV->test(static_cast<boost::uint32_t>(path.front())) ||
          fromAtomsBV->test(static_cast<boost::uint32_t>(path.back()))) {
        keepIt = true;
      }
    }
    if (keepIt && ignoreAtomsBV) {
      BOOST_FOREACH (int pElem, path) {
        if (ignoreAtomsBV->test(pElem)) {
          keepIt = false;
          break;
        }
      }
    }
    if (keepIt) {
      pAtoms.reset();
      for (PATH_TYPE::const_iterator pIt = path.begin(); pIt < path.end();
           ++pIt) {
        // look for a cycle that doesn't start at the first atom
        // we can't effectively canonicalize these at the moment
        // (was github #811)
        if (pIt != path.begin() && *pIt != *(path.begin()) && pAtoms[*pIt]) {
          pathCodes.clear();
          break;
        }
        pAtoms.set(*pIt);
        unsigned int code = atomCodes[*pIt] - 1;
        // subtract off the branching number:
        if (pIt != path.begin() && pIt + 1 != path.end()) {
          --code;
        }
        pathCodes.push_back(code);
      }
      if (pathCodes.size()) {
        boost::int64_t code =
            getTopologicalTorsionCode(pathCodes, includeChirality);
        updateElement(*res, code);
      }
    }
  }
  delete fromAtomsBV;
  delete ignoreAtomsBV;

  return res;
}

namespace {
template <typename T>
void TorsionFpCalc(T *res, const ROMol &mol, unsigned int nBits,
                   unsigned int targetSize,
                   const std::vector<boost::uint32_t> *fromAtoms,
                   const std::vector<boost::uint32_t> *ignoreAtoms,
                   const std::vector<boost::uint32_t> *atomInvariants,
                   bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  std::vector<boost::uint32_t> atomCodes;
  atomCodes.reserve(mol.getNumAtoms());
  for (ROMol::ConstAtomIterator atomItI = mol.beginAtoms();
       atomItI != mol.endAtoms(); ++atomItI) {
    if (!atomInvariants) {
      atomCodes.push_back(getAtomCode(*atomItI, 0, includeChirality));
    } else {
      // need to add to the atomCode here because we subtract off up to 2 below
      // as part of the branch correction
      atomCodes.push_back(((*atomInvariants)[(*atomItI)->getIdx()] << 1) + 1);
    }
  }

  boost::dynamic_bitset<> *fromAtomsBV = 0;
  if (fromAtoms) {
    fromAtomsBV = new boost::dynamic_bitset<>(mol.getNumAtoms());
    BOOST_FOREACH (boost::uint32_t fAt, *fromAtoms) { fromAtomsBV->set(fAt); }
  }
  boost::dynamic_bitset<> *ignoreAtomsBV = 0;
  if (ignoreAtoms) {
    ignoreAtomsBV = new boost::dynamic_bitset<>(mol.getNumAtoms());
    BOOST_FOREACH (boost::uint32_t fAt, *ignoreAtoms) {
      ignoreAtomsBV->set(fAt);
    }
  }

  PATH_LIST paths = findAllPathsOfLengthN(mol, targetSize, false);
  for (PATH_LIST::const_iterator pathIt = paths.begin(); pathIt != paths.end();
       ++pathIt) {
    bool keepIt = true;
    if (fromAtomsBV) {
      keepIt = false;
    }
    const PATH_TYPE &path = *pathIt;
    if (fromAtomsBV) {
      if (fromAtomsBV->test(static_cast<boost::uint32_t>(path.front())) ||
          fromAtomsBV->test(static_cast<boost::uint32_t>(path.back()))) {
        keepIt = true;
      }
    }
    if (keepIt && ignoreAtomsBV) {
      BOOST_FOREACH (int pElem, path) {
        if (ignoreAtomsBV->test(pElem)) {
          keepIt = false;
          break;
        }
      }
    }
    if (keepIt) {
      std::vector<boost::uint32_t> pathCodes(targetSize);
      for (unsigned int i = 0; i < targetSize; ++i) {
        unsigned int code = atomCodes[path[i]] - 1;
        // subtract off the branching number:
        if (i > 0 && i < targetSize - 1) {
          --code;
        }
        pathCodes[i] = code;
      }
      size_t bit = getTopologicalTorsionHash(pathCodes);
      updateElement(*res, bit % nBits);
    }
  }
  delete fromAtomsBV;
  delete ignoreAtomsBV;
}
}  // end of local namespace
SparseIntVect<boost::int64_t> *getHashedTopologicalTorsionFingerprint(
    const ROMol &mol, unsigned int nBits, unsigned int targetSize,
    const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants, bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  SparseIntVect<boost::int64_t> *res = new SparseIntVect<boost::int64_t>(nBits);
  TorsionFpCalc(res, mol, nBits, targetSize, fromAtoms, ignoreAtoms,
                atomInvariants, includeChirality);
  return res;
}

ExplicitBitVect *getHashedTopologicalTorsionFingerprintAsBitVect(
    const ROMol &mol, unsigned int nBits, unsigned int targetSize,
    const std::vector<boost::uint32_t> *fromAtoms,
    const std::vector<boost::uint32_t> *ignoreAtoms,
    const std::vector<boost::uint32_t> *atomInvariants,
    unsigned int nBitsPerEntry, bool includeChirality) {
  PRECONDITION(!atomInvariants || atomInvariants->size() >= mol.getNumAtoms(),
               "bad atomInvariants size");
  static int bounds[4] = {1, 2, 4, 8};
  unsigned int blockLength = nBits / nBitsPerEntry;
  SparseIntVect<boost::int64_t> *sres =
      new SparseIntVect<boost::int64_t>(blockLength);
  TorsionFpCalc(sres, mol, blockLength, targetSize, fromAtoms, ignoreAtoms,
                atomInvariants, includeChirality);
  ExplicitBitVect *res = new ExplicitBitVect(nBits);

  if (nBitsPerEntry != 4) {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second > static_cast<int>(i))
          res->setBit(val.first * nBitsPerEntry + i);
      }
    }
  } else {
    BOOST_FOREACH (SparseIntVect<boost::int64_t>::StorageType::value_type val,
                   sres->getNonzeroElements()) {
      for (unsigned int i = 0; i < nBitsPerEntry; ++i) {
        if (val.second >= bounds[i]) {
          res->setBit(val.first * nBitsPerEntry + i);
        }
      }
    }
  }
  delete sres;
  return res;
}
}  // end of namespace AtomPairs
}  // end of namespace RDKit