#pragma once

#include <Eigen/Core>

#include <ompl/base/spaces/DiscreteStateSpace.h>
#include <ompl/base/ProjectionEvaluator.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/tools/config/MagicConstants.h>

#include <steering_functions/dubins_state_space/dubins_state_space.hpp>
#include <steering_functions/hc_cc_state_space/cc00_reeds_shepp_state_space.hpp>
#include <steering_functions/hc_cc_state_space/cc_dubins_state_space.hpp>
#include <steering_functions/hc_cc_state_space/hc_cc_state_space.hpp>
#include <steering_functions/hc_cc_state_space/hc_reeds_shepp_state_space.hpp>
#include <steering_functions/reeds_shepp_state_space/reeds_shepp_state_space.hpp>

namespace ob = ompl::base;

namespace hc_cc_spaces {
/**
 * OMPL State Space wrapper for HC/CC state spaces (steer functions).
 */
template <class HC_CC_SPACE, unsigned TYPE>
class CurvatureStateSpace : public ob::CompoundStateSpace {
 public:
  class StateType : public CompoundStateSpace::StateType {
   public:
    StateType() = default;

    double getX() const {
      return as<ob::RealVectorStateSpace::StateType>(0)->values[0];
    }

    double getY() const {
      return as<ob::RealVectorStateSpace::StateType>(0)->values[1];
    }

    double getYaw() const { return as<ob::SO2StateSpace::StateType>(1)->value; }

    void setX(double x) {
      as<ob::RealVectorStateSpace::StateType>(0)->values[0] = x;
    }

    void setY(double y) {
      as<ob::RealVectorStateSpace::StateType>(0)->values[1] = y;
    }

    void setXY(double x, double y) {
      setX(x);
      setY(y);
    }

    void setYaw(double yaw) {
      as<ob::SO2StateSpace::StateType>(1)->value = yaw;
    }

    /**
     * Curvature at position (x, y).
     */
    double getKappa() const {
      return as<ob::RealVectorStateSpace::StateType>(2)->values[0];
    }

    /**
     * Driving direction {-1, 0, 1}.
     */
    double getD() const {
      return as<ob::DiscreteStateSpace::StateType>(3)->value;
    }

    void setKappa(double v) {
      as<ob::RealVectorStateSpace::StateType>(2)->values[0] = v;
    }

    void setD(double v) {
      as<ob::DiscreteStateSpace::StateType>(3)->value = (int)v;
    }
  };

  CurvatureStateSpace(const HC_CC_SPACE *space) : space_(space) {
    type_ = ob::STATE_SPACE_TYPE_COUNT + TYPE;
    addSubspace(std::make_shared<ob::RealVectorStateSpace>(2), 1.0);
    addSubspace(std::make_shared<ob::SO2StateSpace>(), 0.5);
    addSubspace(std::make_shared<ob::RealVectorStateSpace>(1), 1.0);
    addSubspace(std::make_shared<ob::DiscreteStateSpace>(-1, 1), 1.0);
    lock();
    ob::RealVectorBounds bounds(1);
    bounds.setLow(0.);
    bounds.setHigh(1.);
    as<ob::RealVectorStateSpace>(2)->setBounds(bounds);
  }

  ~CurvatureStateSpace() = default;

  double distance(const ob::State *state1,
                  const ob::State *state2) const override {
    return space_->get_distance(fromOMPL(state1), fromOMPL(state2));
  }

  void setBounds(const ob::RealVectorBounds &bounds) {
    as<ob::RealVectorStateSpace>(0)->setBounds(bounds);
  }

  const ob::RealVectorBounds &getBounds() const {
    return as<ob::RealVectorStateSpace>(0)->getBounds();
  }

  ob::State *allocState() const override {
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
  }

  void freeState(ob::State *state) const override {
    CompoundStateSpace::freeState(state);
  }

  void registerProjections() override {
    class SE2DefaultProjection : public ob::ProjectionEvaluator {
     public:
      SE2DefaultProjection(const StateSpace *space)
          : ProjectionEvaluator(space) {}

      unsigned int getDimension() const override { return 2; }

      void defaultCellSizes() override {
        cellSizes_.resize(2);
        bounds_ = space_->as<ob::SE2StateSpace>()->getBounds();
        cellSizes_[0] = (bounds_.high[0] - bounds_.low[0]) /
                        ompl::magic::PROJECTION_DIMENSION_SPLITS;
        cellSizes_[1] = (bounds_.high[1] - bounds_.low[1]) /
                        ompl::magic::PROJECTION_DIMENSION_SPLITS;
      }

      void project(const ob::State *state,
                   Eigen::Ref<Eigen::VectorXd> projection) const override {
        projection.resize(2);
        projection[0] = state->as<StateType>()->getX();
        projection[1] = state->as<StateType>()->getY();
      }
    };

    registerDefaultProjection(std::make_shared<SE2DefaultProjection>(this));
  }

  void interpolate(const ob::State *from, const ob::State *to, double t,
                   ob::State *state) const override {
    const CCState state1{fromOMPL(from)}, state2{fromOMPL(to)};
    const auto controls = space_->get_controls(state1, state2);
    CCState result = space_->interpolate(state1, controls, t);
    toOMPL(result, state);
  }

 private:
  const HC_CC_SPACE *space_;

  static CCState fromOMPL(const ob::State *state) {
    const auto *s = state->as<StateType>();
    return {s->getX(), s->getY(), s->getYaw(), s->getKappa(), s->getD()};
  }

  static void toOMPL(const CCState &state, ob::State *result) {
    result->as<StateType>()->setXY(state.x, state.y);
    result->as<StateType>()->setYaw(state.theta);
    result->as<StateType>()->setKappa(state.kappa);
    result->as<StateType>()->setD(state.d);
  }
};

class CCDubinsStateSpace
    : public CurvatureStateSpace<CC_Dubins_State_Space, 1> {
 public:
  CCDubinsStateSpace(double kappa, double sigma, double discretization = 0.1,
                     bool forwards = true)
      : CurvatureStateSpace<CC_Dubins_State_Space, 1>(
            new CC_Dubins_State_Space(kappa, sigma, discretization, forwards)) {
    setName("CC_Dubins");
  }
};

class HCReedsSheppStateSpace
    : public CurvatureStateSpace<HC_Reeds_Shepp_State_Space, 2> {
 public:
  HCReedsSheppStateSpace(double kappa, double sigma,
                         double discretization = 0.1)
      : CurvatureStateSpace<HC_Reeds_Shepp_State_Space, 2>(
            new HC_Reeds_Shepp_State_Space(kappa, sigma, discretization)) {
    setName("HC_Reeds_Shepp");
  }
};

class CCReedsSheppStateSpace
    : public CurvatureStateSpace<CC00_Reeds_Shepp_State_Space, 3> {
 public:
  CCReedsSheppStateSpace(double kappa, double sigma,
                         double discretization = 0.1)
      : CurvatureStateSpace<CC00_Reeds_Shepp_State_Space, 3>(
            new CC00_Reeds_Shepp_State_Space(kappa, sigma, discretization)) {
    setName("CC_Reeds_Shepp");
  }
};

class DubinsStateSpace : public CurvatureStateSpace<Dubins_State_Space, 4> {
 public:
  DubinsStateSpace(double kappa, double discretization = 0.1,
                   bool forwards = true)
      : CurvatureStateSpace<Dubins_State_Space, 4>(
            new Dubins_State_Space(kappa, discretization, forwards)) {
    setName("Dubins");
  }
};

class ReedsSheppStateSpace
    : public CurvatureStateSpace<Reeds_Shepp_State_Space, 5> {
 public:
  ReedsSheppStateSpace(double kappa, double discretization = 0.1)
      : CurvatureStateSpace<Reeds_Shepp_State_Space, 5>(
            new Reeds_Shepp_State_Space(kappa, discretization)) {
    setName("Reeds_Shepp");
  }
};
}  // namespace hc_cc_spaces


#define BOOST_TEST_MODULE "test_bond_length_gocontact_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/GoContactInteraction.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

// a test for specialization of ContactInteraction for GoContactPotential.

BOOST_AUTO_TEST_CASE(ContactGoContactInteraction)
{
    mjolnir::LoggerManager::set_default_logger("test_contact_gocontact.log");
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::GoContactPotential<real_type>;
    using interaction_type = mjolnir::ContactInteraction<traits_type, potential_type>;

    constexpr real_type tol = 1e-7;

    const coord_type pos1(1.0, 0.0, 0.0);
    const coord_type pos2(0.0, 0.0, 0.0);
    system_type sys(2, boundary_type{});

    sys.at(0).mass     = 1.0;
    sys.at(1).mass     = 1.0;
    sys.at(0).rmass    = 1.0;
    sys.at(1).rmass    = 1.0;
    sys.at(0).position = pos1;
    sys.at(1).position = pos2;
    sys.at(0).velocity = coord_type(0,0,0);
    sys.at(1).velocity = coord_type(0,0,0);
    sys.at(0).force    = coord_type(0,0,0);
    sys.at(1).force    = coord_type(0,0,0);
    sys.at(0).name     = "X";
    sys.at(1).name     = "X";
    sys.at(0).group    = "NONE";
    sys.at(1).group    = "NONE";

    const real_type k(1.0);
    const real_type native(1.0);

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    const int N = 10000;
    const real_type dx = 0.001;
    for(int i = 1; i < N; ++i)
    {
        const auto dist = 0.6 + i * dx;
        sys.position(0) = coord_type(0.0,  0.0, 0.0);
        sys.position(1) = coord_type(dist, 0.0, 0.0);
        sys.force(0)    = coord_type(0.0, 0.0, 0.0);
        sys.force(1)    = coord_type(0.0, 0.0, 0.0);

        const real_type deriv = potential.derivative(dist);

        interaction.initialize(sys);
        interaction.calc_force(sys);

        const real_type force_strength1 = mjolnir::math::length(sys.force(0));
        const real_type force_strength2 = mjolnir::math::length(sys.force(1));

        BOOST_TEST(force_strength1 == std::abs(deriv), boost::test_tools::tolerance(tol));
        BOOST_TEST(force_strength2 == std::abs(deriv), boost::test_tools::tolerance(tol));

        const auto E_pot = potential.potential(dist);
        const auto E_int = interaction.calc_energy(sys);

        BOOST_TEST(E_pot == E_int, boost::test_tools::tolerance(tol));
    }
}

BOOST_AUTO_TEST_CASE(ContactGoContact_numerical_difference)
{
    mjolnir::LoggerManager::set_default_logger("test_contact_gocontact.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::GoContactPotential<real_type>;
    using interaction_type = mjolnir::ContactInteraction<traits_type, potential_type>;

    const real_type k(1.0);
    const real_type native(std::sqrt(3.0));

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t i = 0; i < 1000; ++i)
    {
        system_type sys(2, boundary_type{});

        sys.at(0).mass  = 1.0;
        sys.at(1).mass  = 1.0;
        sys.at(0).rmass = 1.0;
        sys.at(1).rmass = 1.0;

        sys.at(0).position = coord_type( 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt));
        sys.at(1).position = coord_type( 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.at(0).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(0).force    = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).force    = coord_type( 0.0, 0.0, 0.0);

        sys.at(0).name  = "X";
        sys.at(1).name  = "X";
        sys.at(0).group = "TEST";
        sys.at(1).group = "TEST";

        const auto init = sys;

        constexpr real_type tol = 2e-4;
        constexpr real_type dr  = 1e-5;
        for(std::size_t idx=0; idx<2; ++idx)
        {
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;
                interaction.initialize(sys);

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::X(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::X(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;
                interaction.initialize(sys);

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Y(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Y(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
            {
                // ----------------------------------------------------------------
                // reset positions
                sys = init;
                interaction.initialize(sys);

                // calc U(x-dx)
                const auto E0 = interaction.calc_energy(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc F(x)
                interaction.calc_force(sys);

                mjolnir::math::Z(sys.position(idx)) += dr;

                // calc U(x+dx)
                const auto E1 = interaction.calc_energy(sys);

                // central difference
                const auto dE = (E1 - E0) * 0.5;

                BOOST_TEST(-dE == dr * mjolnir::math::Z(sys.force(idx)),
                           boost::test_tools::tolerance(tol));
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(ContactGoContact_calc_force_and_energy)
{
    mjolnir::LoggerManager::set_default_logger("test_contact_gocontact.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::GoContactPotential<real_type>;
    using interaction_type = mjolnir::ContactInteraction<traits_type, potential_type>;

    const real_type k(1.0);
    const real_type native(std::sqrt(3.0));

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    std::mt19937 mt(123456789);
    std::uniform_real_distribution<real_type> uni(-1.0, 1.0);

    for(std::size_t i = 0; i < 1000; ++i)
    {
        system_type sys(2, boundary_type{});

        sys.at(0).mass  = 1.0;
        sys.at(1).mass  = 1.0;
        sys.at(0).rmass = 1.0;
        sys.at(1).rmass = 1.0;

        sys.at(0).position = coord_type( 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt), 0.0 + 0.01 * uni(mt));
        sys.at(1).position = coord_type( 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt), 1.0 + 0.01 * uni(mt));
        sys.at(0).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).velocity = coord_type( 0.0, 0.0, 0.0);
        sys.at(0).force    = coord_type( 0.0, 0.0, 0.0);
        sys.at(1).force    = coord_type( 0.0, 0.0, 0.0);

        sys.at(0).name  = "X";
        sys.at(1).name  = "X";
        sys.at(0).group = "TEST";
        sys.at(1).group = "TEST";

        constexpr real_type tol = 1e-4;
        auto ref_sys = sys;

        const auto energy = interaction.calc_force_and_energy(sys);
        const auto ref_energy = interaction.calc_energy(ref_sys);
        interaction.calc_force(ref_sys);
        BOOST_TEST(ref_energy == energy, boost::test_tools::tolerance(tol));

        for(std::size_t idx=0; idx<sys.size(); ++idx)
        {
            BOOST_TEST(mjolnir::math::X(sys.force(idx)) == mjolnir::math::X(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Y(sys.force(idx)) == mjolnir::math::Y(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
            BOOST_TEST(mjolnir::math::Z(sys.force(idx)) == mjolnir::math::Z(ref_sys.force(idx)), boost::test_tools::tolerance(tol));
        }
    }
}


#include <graphene/singularity/scan.hpp>
#include <queue>
#include <boost/graph/graphviz.hpp>

using namespace boost;
using namespace boost::numeric::ublas;
using namespace singularity;

scan::scan(double parameter_e, uint parameter_m) {
    this->parameter_e = parameter_e;
    this->parameter_m = parameter_m;
}

void scan::process(Graph& g) {
    calculate_similarity(g);
    calculate_neighbours(g);
    find_clusters(g);
}

void scan::calculate_neighbours(Graph& g)
{
    calculate_neighbours_partial(g, range_t(0, g.m_vertices.size()));
}

void scan::calculate_neighbours_partial(Graph& g, range_t r) {
    Graph::vertex_iterator current, end;
    tie(current, end) = vertices(g);
    for (;current != end; current++) {
        unsigned int id = get(vertex_index, g, *current);
        if (id >= r.start() && id < r.start() + r.size()) {
            calculate_neighbours(g, *current);
        }
    }
}

void scan::calculate_neighbours(Graph& g, Graph::vertex_descriptor vertex)
{
    Graph::out_edge_iterator current, end;
    tie(current, end) =  out_edges(vertex, g);
    unsigned int neighbours_count = 0;
    for(; current != end; current++) {
        bool structural_similarity_is_high = get(edge_similarity_is_high, g, *current);
        if (structural_similarity_is_high) {
            neighbours_count++;
        }
    }

    bool is_core;

    if (neighbours_count >= parameter_m) {
        is_core = true;
    } else {
        is_core = false;
    }
    put(vertex_is_core, g, vertex, is_core);
    put(vertex_neighbour_count, g, vertex, neighbours_count);
}

void scan::calculate_similarity(Graph& g)
{
    calculate_similarity_partial(g, range_t(0,g.m_edges.size()));
}

void scan::calculate_similarity_partial(Graph& g, range_t r)
{
    Graph::edge_iterator current, end;
    tie(current, end) = edges(g);
    for (;current != end; current++) {
        unsigned int id = get(edge_index, g, *current);
        if (id >= r.start() && id < r.start() + r.size()) {
            calculate_similarity(g, *current);
        }
    }
}

void scan::calculate_similarity(Graph &g, Graph::edge_descriptor link)
{
    double parameter_e_sq = parameter_e * parameter_e;

    Graph::vertex_descriptor v1 = source(link, g), v2 = target(link, g);

    Graph::degree_size_type v_1_N=0, v_2_N=0, v_12_N=0;

    v_1_N = out_degree(v1, g) + 1;
    v_2_N = out_degree(v2, g) + 1;
    v_12_N = 2;

    Graph::adjacency_iterator current_it, end_it;

    tie( current_it, end_it ) = adjacent_vertices(v1, g);

    for (; current_it < end_it; current_it++) {
        bool found;
        Graph::edge_descriptor link2x;
        Graph::vertex_descriptor vx = *current_it;
        tie(link2x, found) = edge(v2, vx, g);
        if (found) {
            v_12_N++;
        }
    }

    double similarity = (double) (v_12_N * v_12_N) / (v_1_N * v_2_N);

    put(edge_similarity, g, link, similarity);
    put(edge_similarity_is_high, g, link, similarity >= parameter_e_sq);
}

void scan::find_clusters(Graph& g)
{
    unsigned int new_cluster_id = 0;

    std::queue<Graph::vertex_descriptor> q;

    auto vertex_cluster_id_map = get(vertex_cluster_id, g);
    auto vertex_status_map = get(vertex_status, g);
    auto vertex_is_core_map = get(vertex_is_core, g);
    auto edge_similarity_is_high_map = get(edge_similarity_is_high, g);

    Graph::vertex_iterator current, end;

    tie(current, end) = vertices(g);

    for ( ; current != end; current++ ) {
        vertex_status_map[*current] = node_status_unclassified;
    }

    tie(current, end) = vertices(g);

    id_generator gen;

    for ( ; current != end; current++ ) {
        Graph::vertex_descriptor v =  *current;

        if (vertex_status_map[v] != node_status_unclassified) {
            continue;
        }

        if (vertex_is_core_map[v]) {
            new_cluster_id = gen.get_next_id();
            vertex_cluster_id_map[v] = new_cluster_id;
            vertex_status_map[v] = node_status_member;
            q.push(v);

            while (q.size() > 0) {
                Graph::vertex_descriptor y = q.front();

                if (vertex_is_core_map[y]) {
                    Graph::out_edge_iterator current_edge, end_edge;
                    tie(current_edge, end_edge) = out_edges(y, g);
                    for (; current_edge != end_edge; current_edge++ ) {
                        bool similarity_is_high = edge_similarity_is_high_map[*current_edge];
                        if (similarity_is_high) {
                            Graph::vertex_descriptor x = target(*current_edge, g);
                            node_status_t status = vertex_status_map[x];
                            if (status == node_status_unclassified || status == node_status_non_member) {
                                vertex_cluster_id_map[x] = new_cluster_id;
                                vertex_status_map[x] = node_status_member;
                            }
                            if (status == node_status_unclassified) {
                                q.push(x);
                            }
                        }
                    }
                }

                q.pop();
            }
        } else {
            vertex_status_map[v] = node_status_non_member;
        }
    }

    tie(current, end) = vertices(g);

    for ( ; current != end; current++ ) {
        Graph::vertex_descriptor v =  *current;
        node_status_t status = vertex_status_map[v];
        if (status == node_status_non_member) {
            new_cluster_id = gen.get_next_id();
            vertex_cluster_id_map[v] = new_cluster_id;
            bool cluster_is_found = false;
            unsigned int found_cluster_id;
            bool node_is_hub = false;
            Graph::out_edge_iterator current_edge, end_edge;
            tie(current_edge, end_edge) = out_edges(v, g);
            for (; current_edge != end_edge; current_edge++ ) {
                Graph::vertex_descriptor x = target(*current_edge, g);
                if (vertex_status_map[x] == node_status_member) {
                    if (!cluster_is_found) {
                        cluster_is_found = true;
                        found_cluster_id = vertex_cluster_id_map[x];
                    } else {
                        if (found_cluster_id != vertex_cluster_id_map[x]) {
                            node_is_hub = true;
                            break;
                        }
                    }
                }
            }

            vertex_status_map[v] = node_is_hub ? node_status_hub : node_status_outlier;
        }
    }

    set_property(g, graph_num_clusters, new_cluster_id + 1);
}

id_generator::id_generator() {
    init();
}

void id_generator::init() {
    current_id = 0;
}

unsigned int id_generator::get_next_id() {
    return current_id ++;
}

void scan::print_graph(Graph& g) {

//
//    dynamic_properties dp;
//
//
//    Graph::vertex_iterator current, end;
//
//    tie (current, end) = vertices(g);
//    for (; current != end; current++) {
//        unsigned int index = get(vertex_index, g, *current);
//        unsigned int cluster_id = get(vertex_cluster_id, g, *current);
//        node_status_t status = get(vertex_status, g, *current);
//        std::string name;
//        switch (status) {
//            case node_status_member:
//                name = std::string("member");
//                break;
//            case node_status_hub:
//                name = std::string("hub");
//                break;
//            case node_status_outlier:
//                name = std::string("outlier");
//                break;
//            default:
//                name = std::string("?");
//        }
//
//        std::string format = std::string("%d (%d) %s");
//        put(vertex_name, g, *current, name);
//    }
//
//    dp.property("similarity", boost::get(edge_similarity, g));
//    dp.property("neighbours", boost::get(vertex_neighbour_count, g));
//    dp.property("node_id", boost::get(vertex_index, g));
//    dp.property("is_core", boost::get(vertex_is_core, g));
//    dp.property("cluster_id", boost::get(vertex_cluster_id, g));
//    dp.property("status", boost::get(vertex_name, g));
//
//    write_graphviz_dp(std::cout, g, dp);

    property_map<Graph, vertex_cluster_id_t>::type vertex_cluster_id_map = get(vertex_cluster_id, g);
    property_map<Graph, vertex_index_t>::type vertex_id_map = get(vertex_index, g);
    property_map<Graph, edge_similarity_t>::type edge_similarity_map = get(edge_similarity, g);

    dynamic_properties dp;

    dp.property("cluster_id", vertex_cluster_id_map);
    dp.property("id", vertex_id_map);
    dp.property("similarity", edge_similarity_map);

    write_graphviz_dp(std::cout, g, dp, "id");
}


/*
 * Copyright (c) 2020, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Compute costs for each player associated with this set of strategies and
// operating point.
//
///////////////////////////////////////////////////////////////////////////////

#include <ilqgames/cost/player_cost.h>
#include <ilqgames/dynamics/multi_player_flat_system.h>
#include <ilqgames/dynamics/multi_player_integrable_system.h>
#include <ilqgames/utils/operating_point.h>
#include <ilqgames/utils/quadratic_cost_approximation.h>
#include <ilqgames/utils/strategy.h>
#include <ilqgames/utils/types.h>

#include <glog/logging.h>
#include <Eigen/Dense>
#include <random>
#include <vector>

namespace ilqgames {

// Compute cost of a set of strategies for each player.
std::vector<float> ComputeStrategyCosts(
    const std::vector<PlayerCost>& player_costs,
    const std::vector<Strategy>& strategies,
    const OperatingPoint& operating_point,
    const MultiPlayerIntegrableSystem& dynamics, const VectorXf& x0,
    float time_step, bool open_loop) {
  // Start at the initial state.
  VectorXf x(x0);
  Time t = 0.0;

  // Walk forward along the trajectory and accumulate total cost.
  std::vector<VectorXf> us(dynamics.NumPlayers());
  std::vector<float> total_costs(dynamics.NumPlayers(), 0.0);
  const size_t num_time_steps =
      (open_loop) ? strategies[0].Ps.size() - 1 : strategies[0].Ps.size();
  for (size_t kk = 0; kk < num_time_steps; kk++) {
    // Update controls.
    for (PlayerIndex ii = 0; ii < dynamics.NumPlayers(); ii++) {
      if (open_loop)
        us[ii] = strategies[ii](kk, VectorXf::Zero(x.size()),
                                operating_point.us[kk][ii]);
      else
        us[ii] = strategies[ii](kk, x - operating_point.xs[kk],
                                operating_point.us[kk][ii]);
    }

    const VectorXf next_x = dynamics.Integrate(t, time_step, x, us);
    const Time next_t = t + time_step;

    // Update costs.
    for (PlayerIndex ii = 0; ii < dynamics.NumPlayers(); ii++) {
      const float cost =
          (open_loop) ? player_costs[ii].EvaluateOffset(t, next_t, next_x, us)
                      : player_costs[ii].Evaluate(t, x, us);

      total_costs[ii] += cost;
    }

    // Update state and time
    x = next_x;
    t = next_t;
  }

  return total_costs;
}

}  // namespace ilqgames


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 2019/12/24 20:41:23
 * Powered by Visual Studio Code
 */
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <vector>
#include <string>
#include <complex>
#include <tuple>
#include <queue>
#include <stack>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <bitset>
#include <functional>
#include <random>
#include <chrono>
#include <cctype>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
// ----- boost -----
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using boost::rational;
using namespace std;
using ll = long long;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{x % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
ll gcd(ll x, ll y) { return y ? gcd(y, x % y) : x; }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes(int i)
{
  cout << i << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}
// ----- main() -----

int H, W;
vector<string> S;

void stamp(int x, int y)
{

#if DEBUG == 1
  cerr << "stamp(" << x << ", " << y << ")" << endl;
#endif
  for (auto i = x; i < min(H, x + H / 2); i++)
  {
    for (auto j = y; j < min(W, y + W / 2); j++)
    {
      S[i][j] = '#';
#if DEBUG == 1
      cerr << "S[" << i << "][" << j << "]" << endl;
#endif
    }
  }
}

int main()
{
  cin >> H >> W;
  S.resize(H);
  for (auto i = 0; i < H; i++)
  {
    cin >> S[i];
  }
  set<int> X, Y;
  for (auto i = 0; i < H; i++)
  {
    for (auto j = 0; j < W; j++)
    {
      if (S[i][j] == '.')
      {
        X.insert(i);
        Y.insert(j);
      }
    }
  }
  if (X.empty())
  {
    Yes(0);
  }
  auto it = X.begin();
  int ubh{*it};
  it = X.end();
  it--;
  int lbh{*it};
  it = Y.begin();
  int ubw{*it};
  it = Y.end();
  it--;
  int lbw{*it};
  int height{lbh - ubh + 1};
  int width{lbw - ubw + 1};
  if (height <= H / 2 && width <= W / 2)
  {
    Yes(1);
  }
  if (height <= H / 2)
  {
    Yes(2);
  }
  if (width <= W / 2)
  {
    Yes(2);
  }
  int cnt{0};
  for (auto k = 0; k < H + W - 1; k++)
  {
    for (auto i = 0; i <= k; i++)
    {
      auto j = k - i;
      if (!(i < H && j < W))
      {
        continue;
      }
      if (0 <= i && i < H && 0 <= j && j < W && S[i][j] == '.')
      {
        ++cnt;
        stamp(i, j);
      }
      auto ni = H - 1 - i;
      auto nj = W - 1 - j;
      if (0 <= ni && ni < H && 0 <= j && j < W && S[ni][j] == '.')
      {
        ++cnt;
        stamp(ni - H / 2 + 1, j);
      }
      if (0 <= i && i < H && 0 <= nj && nj < W && S[i][nj] == '.')
      {
        ++cnt;
        stamp(i, nj - W / 2 + 1);
      }
      if (0 <= ni && ni < H && 0 <= nj && nj < W && S[ni][nj] == '.')
      {
        ++cnt;
        stamp(ni - H / 2 + 1, nj - W / 2 + 1);
      }
    }
  }
  if (cnt > 4)
  {
    assert(false);
  }
  cout << cnt << endl;
}


// Copyright (c) 2014-2016 The Bitcoin Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <net.h>
#include <validation.h>

#include <catch_tests/test_bitcoin.h>

#include "catch_unit.h"
#include <boost/signals2/signal.hpp>

// BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockRewards(const Consensus::Params &consensusParams) {
  const int maxYears = 4;
  const int nInitialReward = consensusParams.nInitialMiningRewardInCoins;
  const int nPeakReward = 1.5*consensusParams.nInitialMiningRewardInCoins;
  const int nBlocksPerYear = consensusParams.nBlocksPerYear;
  Amount nInitialSubsidy = nInitialReward * COIN;

  for (int n = 0; n < maxYears; n++) {
    int nHeight = n * nBlocksPerYear;
    Amount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
    if (n < 1)
      BOOST_CHECK(nSubsidy >= nInitialSubsidy);
    else
      BOOST_CHECK(nSubsidy <= 2 * nInitialReward * COIN);
  }
  // Peak is 0.5 years out. Check here
  BOOST_CHECK_EQUAL(GetBlockSubsidy(0.5 * nBlocksPerYear, consensusParams), nPeakReward * COIN);
  // Check next block reward drops
  BOOST_CHECK(GetBlockSubsidy((0.5 * nBlocksPerYear) + 1, consensusParams) < 2 * nInitialReward * COIN);
}

TEST_CASE("block_subsidy_test") {
  const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
  TestBlockRewards(chainParams->GetConsensus()); // As in main
}

TEST_CASE("subsidy_limit_test") {
  const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
  const int nInitialReward = chainParams->GetConsensus().nInitialMiningRewardInCoins;
  Amount nSum = Amount::zero();
  for (int nHeight = 0; nHeight < 14000000; nHeight += 100000) {
    Amount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
    BOOST_CHECK(nSubsidy <= 2 * nInitialReward * COIN);
    nSum += nSubsidy;
    BOOST_CHECK(MoneyRange(nSum));
  }
  BOOST_CHECK_EQUAL(nSum, Amount(558400000000LL));
}
// BOOST_AUTO_TEST_SUITE_END()


/*
 [auto_generated]
 libs/numeric/odeint/test/step_size_limitation.cpp

 [begin_description]
 Tests the step size limitation functionality
 [end_description]

 Copyright 2015 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#define BOOST_TEST_MODULE odeint_integrate_times

#include <boost/test/unit_test.hpp>

#include <utility>
#include <iostream>
#include <vector>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;
typedef std::vector< value_type > state_type;

/***********************************************
 * first part of the tests: explicit methods
 ***********************************************
 */

void damped_osc( const state_type &x , state_type &dxdt , const value_type t )
{
    const value_type gam( 0.1);

    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam*x[1];
}

struct push_back_time
{
    std::vector< double >& m_times;

    push_back_time( std::vector< double > &times )
    :  m_times( times ) { }

    template<typename State>
    void operator()( const State &x , double t )
    {
        m_times.push_back( t );
    }
};

BOOST_AUTO_TEST_SUITE( step_size_limitation_test )

BOOST_AUTO_TEST_CASE( test_step_adjuster )
{
    // first use adjuster without step size limitation
    default_step_adjuster<double, double> step_adjuster;
    const double dt = 0.1;
    double dt_new = step_adjuster.decrease_step(dt, 1.5, 2);
    BOOST_CHECK(dt_new < dt*2.0/3.0);

    dt_new = step_adjuster.increase_step(dt, 0.8, 1);
    // for errors > 0.5 no increase is performed
    BOOST_CHECK(dt_new == dt);

    dt_new = step_adjuster.increase_step(dt, 0.4, 1);
    // smaller errors should lead to step size increase
    std::cout << dt_new << std::endl;
    BOOST_CHECK(dt_new > dt);

    // now test with step size limitation max_dt = 0.1
    default_step_adjuster<double, double>
        limited_adjuster(dt);

    dt_new = limited_adjuster.decrease_step(dt, 1.5, 2);
    // decreasing works as before
    BOOST_CHECK(dt_new < dt*2.0/3.0);

    dt_new = limited_adjuster.decrease_step(2*dt, 1.1, 2);
    // decreasing a large step size should give max_dt
    BOOST_CHECK(dt_new == dt);

    dt_new = limited_adjuster.increase_step(dt, 0.8, 1);
    // for errors > 0.5 no increase is performed, still valid
    BOOST_CHECK(dt_new == dt);

    dt_new = limited_adjuster.increase_step(dt, 0.4, 1);
    // but even for smaller errors, we should at most get 0.1
    BOOST_CHECK_EQUAL(dt_new, dt);

    dt_new = limited_adjuster.increase_step(0.9*dt, 0.1, 1);
    std::cout << dt_new << std::endl;
    // check that we don't increase beyond max_dt
    BOOST_CHECK(dt_new == dt);
}

template<class Stepper>
void test_explicit_stepper(Stepper stepper, const double max_dt)
{
    state_type x( 2 );
    x[0] = x[1] = 10.0;
    const size_t steps = 100;

    std::vector<double> times;

    integrate_adaptive(stepper, damped_osc, x, 0.0, steps*max_dt, max_dt, push_back_time(times));

    BOOST_CHECK_EQUAL(times.size(), steps+1);
    // check that dt remains at exactly max_dt
    for( size_t i=0 ; i<times.size() ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_SMALL( times[i] - static_cast<double>(i)*max_dt , 1E-15);
    times.clear();

    // this should also work when we provide some bigger initial dt
    integrate_adaptive(stepper, damped_osc, x, 0.0, steps*max_dt, 10*max_dt, push_back_time(times));

    BOOST_CHECK_EQUAL(times.size(), steps+1);
    // check that dt remains at exactly max_dt
    for( size_t i=0 ; i<times.size() ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_SMALL( times[i] - static_cast<double>(i)*max_dt , 1E-15);
    times.clear();
}

BOOST_AUTO_TEST_CASE( test_controlled )
{
    const double max_dt = 0.01;

    test_explicit_stepper(make_controlled(1E-2, 1E-2, max_dt,
                                            runge_kutta_dopri5<state_type>()),
                            max_dt);
    test_explicit_stepper(make_controlled(1E-2, 1E-2, -max_dt,
                                            runge_kutta_dopri5<state_type>()),
                            -max_dt);

    test_explicit_stepper(make_controlled(1E-2, 1E-2, max_dt,
                                            runge_kutta_cash_karp54<state_type>()),
                            max_dt);
    test_explicit_stepper(make_controlled(1E-2, 1E-2, -max_dt,
                                            runge_kutta_cash_karp54<state_type>()),
                            -max_dt);

    test_explicit_stepper(bulirsch_stoer<state_type>(1E-2, 1E-2, 1.0, 1.0, max_dt),
                            max_dt);
    test_explicit_stepper(bulirsch_stoer<state_type>(1E-2, 1E-2, 1.0, 1.0, -max_dt),
                            -max_dt);
}

BOOST_AUTO_TEST_CASE( test_dense_out )
{
    const double max_dt = 0.01;
    test_explicit_stepper(make_dense_output(1E-2, 1E-2, max_dt,
                                             runge_kutta_dopri5<state_type>()),
                          max_dt);
    test_explicit_stepper(make_dense_output(1E-2, 1E-2, -max_dt,
                                            runge_kutta_dopri5<state_type>()),
                          -max_dt);

    test_explicit_stepper(bulirsch_stoer_dense_out<state_type>(1E-2, 1E-2, 1, 1, max_dt),
                          max_dt);

    test_explicit_stepper(bulirsch_stoer_dense_out<state_type>(1E-2, 1E-2, 1, 1, -max_dt),
                          -max_dt);
}

/***********************************************
 * second part of the tests: implicit Rosenbrock
 ***********************************************
 */

typedef boost::numeric::ublas::vector< value_type > vector_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;

// harmonic oscillator, analytic solution x[0] = sin( t )
struct osc_rhs
{
    void operator()( const vector_type &x , vector_type &dxdt , const value_type &t ) const
    {
        dxdt( 0 ) = x( 1 );
        dxdt( 1 ) = -x( 0 );
    }
};

struct osc_jacobi
{
    void operator()( const vector_type &x , matrix_type &jacobi , const value_type &t , vector_type &dfdt ) const
    {
        jacobi( 0 , 0 ) = 0;
        jacobi( 0 , 1 ) = 1;
        jacobi( 1 , 0 ) = -1;
        jacobi( 1 , 1 ) = 0;
        dfdt( 0 ) = 0.0;
        dfdt( 1 ) = 0.0;
    }
};

template<class Stepper>
void test_rosenbrock_stepper(Stepper stepper, const double max_dt)
{
    vector_type x( 2 );
    x(0) = x(1) = 10.0;
    const size_t steps = 100;

    std::vector<double> times;

    integrate_adaptive(stepper,
                       std::make_pair(osc_rhs(), osc_jacobi()),
                       x, 0.0, steps*max_dt, max_dt, push_back_time(times));

    BOOST_CHECK_EQUAL(times.size(), steps+1);
    // check that dt remains at exactly max_dt
    for( size_t i=0 ; i<times.size() ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_SMALL( times[i] - static_cast<double>(i)*max_dt , 1E-15);
    times.clear();

    // this should also work when we provide some bigger initial dt
    integrate_adaptive(stepper,
                       std::make_pair(osc_rhs(), osc_jacobi()),
                       x, 0.0, steps*max_dt, 10*max_dt, push_back_time(times));

    BOOST_CHECK_EQUAL(times.size(), steps+1);
    // check that dt remains at exactly max_dt
    for( size_t i=0 ; i<times.size() ; ++i )
        // check if observer was called at times 0,1,2,...
        BOOST_CHECK_SMALL( times[i] - static_cast<double>(i)*max_dt , 1E-15);
    times.clear();
}

BOOST_AUTO_TEST_CASE( test_controlled_rosenbrock )
{
    const double max_dt = 0.01;

    test_rosenbrock_stepper(make_controlled(1E-2, 1E-2, max_dt, rosenbrock4<value_type>()),
                            max_dt);
    test_rosenbrock_stepper(make_controlled(1E-2, 1E-2, -max_dt, rosenbrock4<value_type>()),
                            -max_dt);
}

BOOST_AUTO_TEST_CASE( test_dense_out_rosenbrock )
{
    const double max_dt = 0.01;

    test_rosenbrock_stepper(make_dense_output(1E-2, 1E-2, max_dt, rosenbrock4<value_type>()),
                            max_dt);
    test_rosenbrock_stepper(make_dense_output(1E-2, 1E-2, -max_dt, rosenbrock4<value_type>()),
                            -max_dt);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <string>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <Eigen/Geometry>

#include <boost/format.hpp>     // for formating strings

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main(int argc, char** argv)
{
    std::vector<cv::Mat> colorImgs, depthImgs;
//     vector<Eigen::Isometry3d> poses;
    std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;

    ifstream fin("../../data/pose.txt");
    if (!fin) {
        cerr << "please run the program with the directory -'../../' and file 'pose.txt'.";
        return 1;
    }

    for (int i = 0; i < 5; i ++) {
        boost::format fmt("../../%s/%d.%s");
        colorImgs.push_back(cv::imread((fmt%"data/color"%(i+1)%"png").str()));
        depthImgs.push_back(cv::imread((fmt%"data/depth"%(i+1)%"pgm").str(), -1));

        double data[7] = {0};
        for (auto & d:data) {
            fin >> d;
        }
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        Eigen::Isometry3d  T(q);
        T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(T);
    }

    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;

    cout << "The image is being converted to a point cloud..." << endl;

    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    /* Create a new point cloud */
    PointCloud::Ptr pointCloud(new PointCloud);
    for (int i = 0; i < 5; i ++) {
        PointCloud::Ptr current(new PointCloud);

        cout << "Image "<< i+1 << " is being converted ... " << endl;

        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for (int v = 0; v < color.rows; v ++) {
            for (int u = 0; u < color.cols; u ++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u];

                if (d == 0) continue;       /* last measure, value is 0 */
                if (d >= 7000) continue;    /* the depth value too large  */

                Eigen::Vector3d point;
                point[2] = double(d)/depthScale;
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy;
                Eigen::Vector3d pointWorld = T*point;

                PointT p;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[v*color.step + u*color.channels()];
                p.g = color.data[v*color.step + u*color.channels() + 1];
                p.r = color.data[v*color.step + u*color.channels() + 2];

                current->points.push_back(p);
            }
        }

        // depth filter and statistical removal
        PointCloud::Ptr tmp(new PointCloud);
        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
        statistical_filter.setMeanK(50);
        statistical_filter.setStddevMulThresh(1.0);
        statistical_filter.setInputCloud(current);
        statistical_filter.filter(*tmp);
        (*pointCloud) += *tmp;
    }

    pointCloud->is_dense = false;
    cout << "Point cloud total number is : " << pointCloud->size() << endl;

    // voxel filter
    pcl::VoxelGrid<PointT> voxel_filter;
    voxel_filter.setLeafSize(0.01, 0.01, 0.01);     // resolution
    PointCloud::Ptr tmp(new PointCloud);
    voxel_filter.setInputCloud(pointCloud);
    voxel_filter.filter(*tmp);
    tmp->swap(*pointCloud);

    cout << "filter after, the point cloud total number is : " << pointCloud->size() << std::endl;

    pcl::io::savePCDFileBinary("map.pcd", *pointCloud);

    return 0;
}


#include "Dynamics.h"

Dynamics::~Dynamics(){}

std::vector<double> Dynamics::advance(std::vector<double> x, std::vector<double> u, double dt){
	// calls compute_xd() and performs integration with a desired solver
	return x;
}

// http://headmyshoulder.github.io/odeint-v2/examples.html#simple1dexample
// Example: Boost
// #include <iostream>
// #include <boost/array.hpp>

// #include <boost/numeric/odeint.hpp>

// using namespace std;
// using namespace boost::numeric::odeint;

// const double sigma = 10.0;
// const double R = 28.0;
// const double b = 8.0 / 3.0;

// typedef boost::array< double , 3 > state_type;

// void lorenz( const state_type &x , state_type &dxdt , double t )
// {
//     dxdt[0] = sigma * ( x[1] - x[0] );
//     dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
//     dxdt[2] = -b * x[2] + x[0] * x[1];
// }

// void write_lorenz( const state_type &x , const double t )
// {
//     cout << t << '\t' << x[0] << '\t' << x[1] << '\t' << x[2] << endl;
// }

// int main(int argc, char **argv)
// {
//     state_type x = { 10.0 , 1.0 , 1.0 }; // initial conditions
//     integrate( lorenz , x , 0.0 , 25.0 , 0.1 , write_lorenz );
// }

// https://stackoverflow.com/questions/16512817/numerical-integration-in-c
// GNU Scientific Library
// #include <stdio.h>
// #include <math.h>
// #include <gsl/gsl_integration.h>

// double f (double x, void * params) {
//   double alpha = *(double *) params;
//   return log(alpha*x) / sqrt(x);
// }

// int
// main (void)
// {
//   double result, error;
//   double expected = -4.0;
//   double alpha = 1.0;
//   gsl_integration_workspace * w
//     = gsl_integration_workspace_alloc (1000);

//   gsl_function F;
//   F.function = &f;
//   F.params = α

//   gsl_integration_qags (&F, 0, 1, 0, 1e-7, 1000,
//                         w, &result, &error);

//   printf ("result          = % .18f\n", result);
//   printf ("exact result    = % .18f\n", expected);
//   printf ("estimated error = % .18f\n", error);
//   printf ("actual error    = % .18f\n", result - expected);
//   printf ("intervals =  %d\n", w->size);

//   gsl_integration_workspace_free (w);

//   return 0;
// }


//
// Copyright (c) 2017-2019 Native Instruments GmbH, Berlin
//
// 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 <ni/media/pcm/limits.h>
#include <ni/media/pcm/numspace.h>

#include <gtest/gtest.h>

#include <boost/range/algorithm/adjacent_find.hpp>
#include <boost/range/size.hpp>

#include <algorithm>
#include <cmath>
#include <type_traits>

template <class Value>
class limits : public testing::Test
{

public:
protected:
    int bits() const
    {
        static constexpr auto max_bits = 31;
        return std::min( pcm::numeric_limits<Value>::bits(), max_bits );
    }

    auto values() const
    {
        return pcm::numspace<Value>( bits() );
    }

    size_t actual_size() const
    {
        return size_t( boost::size( values() ) );
    }

    size_t expected_size() const
    {
        return size_t( 1 ) << bits();
    }

    bool equidistant() const
    {
        auto step = pcm::numeric_limits<Value>::step( bits() );

        return std::end( values() )
               == boost::adjacent_find( values(), [step]( Value a, Value b ) { return a + step != b; } );
    }

    Value expected_min() const
    {
        return pcm::numeric_limits<Value>::min();
    }

    Value actual_min() const
    {
        return *std::begin( values() );
    }

    Value expected_max() const
    {
        return pcm::numeric_limits<Value>::max( bits() );
    }

    Value actual_max() const
    {
        return *--std::end( values() );
    }

    Value expected_zero() const
    {
        return pcm::numeric_limits<Value>::zero();
    }

    Value actual_zero() const
    {
        return *( std::begin( values() ) + boost::size( values() ) / 2 );
    }
};

TYPED_TEST_CASE_P( limits );

TYPED_TEST_P( limits, expect_equidistant )
{
    // this takes too long
    // EXPECT_TRUE( this->equidistant());
    EXPECT_TRUE( true );
}

TYPED_TEST_P( limits, expect_size )
{
    EXPECT_EQ( this->expected_size(), this->actual_size() );
}

TYPED_TEST_P( limits, expect_min )
{
    EXPECT_EQ( this->expected_min(), this->actual_min() );
}

TYPED_TEST_P( limits, expect_max )
{
    EXPECT_EQ( this->expected_max(), this->actual_max() );
}

TYPED_TEST_P( limits, expect_zero )
{
    EXPECT_EQ( this->expected_zero(), this->actual_zero() );
}

REGISTER_TYPED_TEST_CASE_P( limits, expect_equidistant, expect_size, expect_min, expect_max, expect_zero );

using limits_test_t =
    testing::Types<uint8_t, uint16_t, uint32_t, uint64_t, int8_t, int16_t, int32_t, int64_t, float, double>;

INSTANTIATE_TYPED_TEST_CASE_P( all, limits, limits_test_t );


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_MINDENORMAL_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_MINDENORMAL_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/float.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/config.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_mindenormal Mindenormal
 *
 * \par Description
 * Constant Mindenormal : the least non zero positive value of floating point numbers,
 * i.e. 4.940656458412465e-324 for double and 1.4012985e-45 for float
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/mindenormal.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::mindenormal_(A0)>::type
 *     Mindenormal();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Mindenormal
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Mindenormal of functor Mindenormal
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/

    #ifdef BOOST_SIMD_NO_DENORMALS
    struct Smallestposval;
    typedef Smallestposval Mindenormal;
    #else
    struct Mindenormal : ext::pure_constant_<Mindenormal>
    {
      template<class Target, class Dummy=void>
      struct apply : meta::int_c<typename Target::type,1> {};
    };

    template<class T, class Dummy>
    struct  Mindenormal::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<1> {};

    template<class T, class Dummy>
    struct  Mindenormal::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<1> {};
    #endif
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Mindenormal, Mindenormal)

} }

#ifdef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/constants/smallestposval.hpp>
#endif

#include <boost/simd/sdk/constant/common.hpp>

#endif


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

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
using namespace boost::hana;

int main() {

{

//! [succ]
BOOST_HANA_CONSTANT_CHECK(succ(int_<4>) == int_<5>);
BOOST_HANA_CONSTEXPR_CHECK(succ(10) == 11);
//! [succ]

}{

//! [pred]
BOOST_HANA_CONSTANT_CHECK(pred(int_<4>) == int_<3>);
BOOST_HANA_CONSTEXPR_CHECK(pred(10) == 9);
//! [pred]
}

}


#include <blitz/array.h>

using namespace blitz;

template<class T>
void sink(T&) { }

void foo()
{
    Vector<float> A(100), B(100);

    A = B + B;
    sink(A);
    sink(B);
}


//=====================================================
// File   :  ublas_interface.hh
// Author :  L. Plagne <laurent.plagne@edf.fr)>
// Copyright (C) EDF R&D,  lun sep 30 14:23:27 CEST 2002
//=====================================================
//
// 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 2
// 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, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//
#ifndef UBLAS_INTERFACE_HH
#define UBLAS_INTERFACE_HH

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>

using namespace boost::numeric;

template <class real>
class ublas_interface{

public :

  typedef real real_type ;

  typedef std::vector<real> stl_vector;
  typedef std::vector<stl_vector> stl_matrix;

  typedef typename boost::numeric::ublas::matrix<real,boost::numeric::ublas::column_major> gene_matrix;
  typedef typename boost::numeric::ublas::vector<real> gene_vector;

  static inline std::string name( void ) { return "ublas"; }

  static void free_matrix(gene_matrix & A, int N) {}

  static void free_vector(gene_vector & B) {}

  static inline void matrix_from_stl(gene_matrix & A, stl_matrix & A_stl){
    A.resize(A_stl.size(),A_stl[0].size());
    for (int j=0; j<A_stl.size() ; j++)
      for (int i=0; i<A_stl[j].size() ; i++)
        A(i,j)=A_stl[j][i];
  }

  static inline void vector_from_stl(gene_vector & B, stl_vector & B_stl){
    B.resize(B_stl.size());
    for (int i=0; i<B_stl.size() ; i++)
      B(i)=B_stl[i];
  }

  static inline void vector_to_stl(gene_vector & B, stl_vector & B_stl){
    for (int i=0; i<B_stl.size() ; i++)
      B_stl[i]=B(i);
  }

  static inline void matrix_to_stl(gene_matrix & A, stl_matrix & A_stl){
    int N=A_stl.size();
    for (int j=0;j<N;j++)
    {
      A_stl[j].resize(N);
      for (int i=0;i<N;i++)
        A_stl[j][i]=A(i,j);
    }
  }

  static inline void copy_vector(const gene_vector & source, gene_vector & cible, int N){
    for (int i=0;i<N;i++){
      cible(i) = source(i);
    }
  }

  static inline void copy_matrix(const gene_matrix & source, gene_matrix & cible, int N){
    for (int i=0;i<N;i++){
      for (int j=0;j<N;j++){
        cible(i,j) = source(i,j);
      }
    }
  }

  static inline void matrix_vector_product_slow(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X =  prod(A,B);
  }

  static inline void matrix_matrix_product_slow(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N){
    X =  prod(A,B);
  }

  static inline void axpy_slow(const real coef, const gene_vector & X, gene_vector & Y, int N){
    Y+=coef*X;
  }

  // alias free assignments

  static inline void matrix_vector_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X.assign(prod(A,B));
  }

  static inline void atv_product(gene_matrix & A, gene_vector & B, gene_vector & X, int N){
    X.assign(prod(trans(A),B));
  }

  static inline void matrix_matrix_product(gene_matrix & A, gene_matrix & B, gene_matrix & X, int N){
    X.assign(prod(A,B));
  }

  static inline void axpy(const real coef, const gene_vector & X, gene_vector & Y, int N){
    Y.plus_assign(coef*X);
  }

  static inline void axpby(real a, const gene_vector & X, real b, gene_vector & Y, int N){
    Y = a*X + b*Y;
  }

  static inline void ata_product(gene_matrix & A, gene_matrix & X, int N){
    // X =  prod(trans(A),A);
    X.assign(prod(trans(A),A));
  }

  static inline void aat_product(gene_matrix & A, gene_matrix & X, int N){
    // X =  prod(A,trans(A));
    X.assign(prod(A,trans(A)));
  }

  static inline void trisolve_lower(const gene_matrix & L, const gene_vector& B, gene_vector & X, int N){
    X = solve(L, B, ublas::lower_tag ());
  }

};

#endif


//ibd_m.cpp equivalent cpp script of ibd_m.py
#include <boost/python.hpp>
#include<iostream>
#include<vector>
#include<algorithm>
#include<map>
#include<string>
using namespace std;
typedef std::vector<float> VecFloat;
typedef std::vector<string> VecString;
typedef std::vector<int> VecInt;
typedef std::vector<int> VecInt;
typedef std::vector<VecInt> VecVecInt;
VecFloat sib_ibd(VecString &geno);
float prob_ibd(VecString &geno, string &allele);
float cousin_ibd(VecString &geno);
float un_ibd(VecString &geno);

VecFloat sib_ibd(VecString &geno)
{
	VecFloat ibd (3,0.0);
	VecVecInt inherit;
	std::vector<VecVecInt> compat_inherit;
	for (VecString::iterator it=geno.begin()+4; it<geno.end(); it++)
	{
		//for each allele in offspring
		string sib_allele = *it;
		VecInt tmp_inheirt;
		VecString::iterator pos=geno.begin()-1;
		while (pos != geno.begin()+4)
		{
			pos = std::find(pos+1,geno.begin()+4,sib_allele);
			if ((pos-geno.begin()) !=4){tmp_inheirt.push_back(pos-geno.begin());}
		}
		inherit.push_back(tmp_inheirt);
	}
	for (int indiv=0; indiv<2; indiv++)
	{
		//for each offspring
		VecVecInt sib_inherit;
		for (VecInt::iterator tmp_it=inherit[2*indiv].begin();tmp_it<inherit[2*indiv].end();tmp_it++)
		{
			for (VecInt::iterator tmp2_it=inherit[2*indiv+1].begin();tmp2_it<inherit[2*indiv+1].end();tmp2_it++)
			{
				if (*tmp_it<2 && *tmp2_it>1 || *tmp_it>1 && *tmp2_it<2)
				{
					VecInt temp {*tmp_it,*tmp2_it};
					sib_inherit.push_back(temp);
				}
			}
		}
		compat_inherit.push_back(sib_inherit);
	}
	float count[3] = {0.0,0.0,0.0};
	int count_sum = compat_inherit[0].size()*compat_inherit[1].size();
	for (VecVecInt::iterator tmp1_it=compat_inherit[0].begin(); tmp1_it<compat_inherit[0].end(); tmp1_it++)
	{
		//count IBD value
		for (VecVecInt::iterator tmp2_it=compat_inherit[1].begin(); tmp2_it<compat_inherit[1].end(); tmp2_it++)
		{
			int ibd_count=0;
			VecInt tmp_common;
			for (VecInt::iterator i = tmp1_it->begin(); i != tmp1_it->begin()+2;i++)
			{
				if (std::find(tmp2_it->begin(),tmp2_it->begin()+2,*i) != tmp2_it->begin()+2 && geno.at(*i) != "1")
				{
					tmp_common.push_back(*i);//shared allele in [0,1,2,3]
				}
			}
			ibd_count = tmp_common.size();
			count[ibd_count]++;
		}
	}
	for (int i=0; i<3; i++){ibd[i]=count[i]/count_sum;}
	return ibd;
}

float prob_ibd(VecString &geno, string &allele)
{
	VecInt pos;   //position of the given allele in parental genotypes
	VecVecInt alt_pos;
	float prob=0.0;
	VecString alt {"-1","-1"};
	for (auto iter=geno.begin(); iter != geno.begin()+4; iter++)
	{
		if (*iter == allele){pos.push_back((iter-geno.begin()));}
	}
	for (auto iter=geno.begin()+4; iter != geno.end(); iter++)
	{
		//determine the alternative allele
		int tmp_pos = iter-(geno.begin()+4);
		if (*iter != allele)
		{
			if (tmp_pos<2){alt[0]=*iter;}
			else {alt[1]=*iter;}
		}
		else if (tmp_pos%2==1 && alt[tmp_pos/2]=="-1"){alt[tmp_pos/2]=*iter;}
	}
	VecInt altpos1, altpos2;
	for (auto iter=geno.begin(); iter != geno.begin()+4; iter++)
	{
		if (*iter == alt[0]){altpos1.push_back((iter-geno.begin()));}
		if (*iter == alt[1]){altpos2.push_back((iter-geno.begin()));}
	}
	alt_pos.push_back(altpos1);
	alt_pos.push_back(altpos2);
	vector<map<int,float>> freq_map;
	for (int i=0;i<2;i++)
	{
		map<int,float> tmp_occurrence;
		VecInt tmp_p_compat;
		for (auto p : pos)
		{
			for (auto q : alt_pos[i])
			{
				if (p<2 && q>1 || p>1 && q<2){tmp_p_compat.push_back(p);}
			}
		}
		for (auto tmp : tmp_p_compat)
		{
			float count = (float) 1/tmp_p_compat.size();
			if(tmp_occurrence.find(tmp)==tmp_occurrence.end())
			{
				//not in key
				tmp_occurrence[tmp]=count;
			}
			else {tmp_occurrence[tmp]+=count;}
		}
		freq_map.push_back(tmp_occurrence);
	}
	for (auto &it1 : freq_map[0])
	{
		for (auto &it2 : freq_map[1])
		{
			if (it2.first==it1.first){prob+=it1.second*it2.second;}
		}
	}
	return prob;
}

float cousin_ibd(VecString &geno)
{
	//calculate IBD between cousins
	//geno = GT for [grandparents, fam1, fam2] important parent put in the first place of each family
	VecString ag = {geno[8],geno[9]};       //GT for 2 cousins
	VecString bg = {geno[14],geno[15]};
	VecString shared_allele;
	float total_p = 0.0;
	for (auto ait : ag)
	{
		if (std::find(bg.begin(),bg.end(),ait) != bg.end() && ait != "1"){shared_allele.push_back(ait);} //possible shared RV
	}
	if (shared_allele.size()==2 && shared_allele[0]==shared_allele[1]){shared_allele.pop_back();}
	for (auto a : shared_allele)
	{
		int rep=0;
		for (auto tmp : ag)
		{
			for (auto tmp1 : bg)
			{
				if (tmp==a && tmp1==a){rep++;}
			}
		}
		VecFloat f_inherit;
		int flag=0;
		for (int fid=0; fid<2; fid++)
		{
			VecVecInt possible_inherit;
			VecString pg = {geno[fid*6+4],geno[fid*6+5],geno[fid*6+6],geno[fid*6+7]};     //parental genotypes
			string alt="-1";
			VecInt pos, pos_alt;
			if (fid==0)
			{
				for (auto ait=ag.begin(); ait != ag.end(); ait++)
				{
					if (*ait != a)
					{
						alt=*ait;
						break;
					}
					else if (ait-ag.begin()==1)
					{
						alt=*ait;
					}
				}
			}
			if (fid==1)
			{
				for (auto bit=bg.begin(); bit != bg.end(); bit++)
				{
					if (*bit != a)
					{
						alt=*bit;
						break;
					}
					else if (bit-bg.begin()==1)
					{
						alt=*bit;
					}
				}
			}
			for (auto it = pg.begin(); it !=pg.end(); it++)
			{
				if (*it == a) {pos.push_back((it-pg.begin()));}
				if (*it == alt) {pos_alt.push_back((it-pg.begin()));}
			}
			for (auto p : pos)
			{
				for (auto q : pos_alt)
				{
					if (p<2 && q>1 || p>1 && q<2)
					{
						VecInt tmp_pos={p,q};
						//cout<<p<<" "<<q<<endl;
						possible_inherit.push_back(tmp_pos);
					}
				}
			}
			int count=0;
			for (auto inherit : possible_inherit)
			{
				if (inherit[0]<2){count++;}
			}
			float f_in;
			if (possible_inherit.size()>0)
			{
				f_in = (float)count/possible_inherit.size();
			}
			else {f_in=0;}
			f_inherit.push_back(f_in);
			if (f_in>0){flag++;}     //family having a possibility to inherit the particular allele
			if (flag==2)
			{
				//get the GT for upper nuclear family
				VecString up_gt={geno[0],geno[1],geno[2],geno[3],geno[4],geno[5],geno[10],geno[11]};
				float prob = prob_ibd(up_gt,a);
				total_p += (f_inherit[0]*f_inherit[1]*prob)*rep;
			}
			else {total_p+=0;}
		}
	}
	return total_p;
}

float un_ibd(VecString &geno)
{
	//calculate IBD between Uncle-Nephew pair
	//geno = GT for [grandparents uncle father mother kid(nephew)]
	VecString Uncle_g = {geno[4],geno[5]};
	VecString Nephew_g = {geno[10],geno[11]};
	VecString pg = {geno[6],geno[7],geno[8],geno[9]};
	VecString gt = {geno[0],geno[1],geno[2],geno[3],geno[4],geno[5],geno[6],geno[7]};
	VecString shared_allele;
	float total_p=0.0;
	for (auto g : Uncle_g)
	{
		if (std::find(Nephew_g.begin(),Nephew_g.end(),g) != Nephew_g.end() && g != "1")
		{
			shared_allele.push_back(g);
		}
	}
	if (shared_allele.size()==2 && shared_allele[0]==shared_allele[1]){shared_allele.pop_back();}
	for (auto ref : shared_allele)
	{
		VecString tmp_alleles;
		for (auto b : Nephew_g)
		{
			if (b==ref){tmp_alleles.push_back(b);}
		}
		for (auto allele : tmp_alleles)
		{
			float f=0.0;
			int m=0;
			int n=0;
			string alt="-1";
			for (auto bit=Nephew_g.begin(); bit != Nephew_g.end(); bit++)
			{
				if (*bit != ref)
				{
					alt=*bit;
					break;
				}
				else if (bit-Nephew_g.begin()==1)
				{
					alt=*bit;
				}
			}
			VecInt pos1,pos2;
			for (auto tmp=pg.begin();tmp!=pg.end();tmp++)
			{
				if (*tmp==ref){pos1.push_back((tmp-pg.begin()));}
				if (*tmp==alt){pos2.push_back((tmp-pg.begin()));}
			}
			for (auto p : pos1)
			{
				for (auto q : pos2)
				{
					if (p<2 && q>1){m++;}
					else if (p>1 && q<2){n++;}
				}
			}
			if ((m+n)>0){f=(float)m/(m+n);}
			else{f=0;}
			float prob=0;
			if (f!=0)
			{
				prob=prob_ibd(gt,ref);
			}
			if (Uncle_g[0]==Uncle_g[1] && Uncle_g[0]==ref)
			{
				prob *=2;
			}
			total_p += f*prob;
		}
	}
	return total_p;
}
boost::python::list sib_apply(boost::python::list geno_py)
{
	VecString geno;
	VecFloat ibd;
	boost::python::list ibd_py;
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	ibd=sib_ibd(geno);
	for (auto iter : ibd)
	{
		ibd_py.append(iter);
	}
	return ibd_py;
}

float prob_ibd_apply(boost::python::list geno_py,int allele)
{
	VecString geno;
	float prob;
	string str_allele = std::to_string(allele);
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	prob = prob_ibd(geno, str_allele);
	return prob;
}

float cousin_apply(boost::python::list geno_py)
{
	VecString geno;
	float prob;
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	prob = cousin_ibd(geno);
	return prob;
}

float un_apply(boost::python::list geno_py)
{
	VecString geno;
	float prob;
	for (int i=0; i<len(geno_py); i++)
	{
		boost::python::extract<int> extracted_geno(geno_py[i]);
		string str_geno = std::to_string(extracted_geno);
		geno.push_back(str_geno);
	}
	prob = un_ibd(geno);
	return prob;
}
BOOST_PYTHON_MODULE(ibd_rv_cpp){
	using namespace boost::python;
	def("sib_ibd",sib_ibd);
	def("sib_apply",sib_apply);
	def("prob_ibd_apply",prob_ibd_apply);
	def("cousin_apply",cousin_apply);
	def("un_apply",un_apply);
}

/*int main()
{
	VecString geno {"1","1","2","1","1","1","2","1","1","1","1","1"};
	float prob=un_ibd(geno);
	cout<<"IBD:"<<prob<<endl;
	return 0;
}
int main(int argc, char* argv[])
{
	VecString geno;
	VecFloat ibd;
	for (char* i=*(argv+1); *i != '\0';i++)
	{
		geno.push_back(string(1,*i));
	}
	ibd=sib_ibd(geno);
}*/


#include "kino_plan/bvp_solver.h"
#include "visualization_utils/visualization_utils.h"
#include "quadrotor_msgs/PositionCommand.h"
#include <ros/ros.h>
#include <Eigen/Eigen>

class BVPTest
{
public:
  BVPTest(const ros::NodeHandle &nh): nh_(nh)
  {
    nh.param("test/start_px", start_state_[0], 0.0);
    nh.param("test/start_py", start_state_[1], 0.0);
    nh.param("test/start_pz", start_state_[2], 0.0);
    nh.param("test/start_vx", start_state_[3], 0.0);
    nh.param("test/start_vy", start_state_[4], 0.0);
    nh.param("test/start_vz", start_state_[5], 0.0);
    nh.param("test/start_ax", start_state_[6], 0.0);
    nh.param("test/start_ay", start_state_[7], 0.0);
    nh.param("test/start_az", start_state_[8], 0.0);
    nh.param("test/rho", rho_, 0.0);

    vis_ptr_.reset(new VisualRviz(nh));
    goal_sub_ = nh_.subscribe("/goal", 1, &BVPTest::goalCallback, this);

    bvp_.init(TRIPLE_INTEGRATOR);
    bvp_.setRho(rho_);
  };

private:
  ros::NodeHandle nh_;
  BVPSolver::IntegratorBVP bvp_;
  VisualRviz::Ptr vis_ptr_;
  StatePVA start_state_, end_state_;
  double rho_;

  ros::Subscriber goal_sub_;
  void goalCallback(const quadrotor_msgs::PositionCommand::ConstPtr &goal_msg)
  {
    end_state_ << goal_msg->position.x,
        goal_msg->position.y,
        goal_msg->position.z,
        goal_msg->velocity.x + 1.0,
        goal_msg->velocity.y + 2.0,
        goal_msg->velocity.z,
        goal_msg->acceleration.x,
        goal_msg->acceleration.y,
        goal_msg->acceleration.z;
    solve(start_state_, end_state_);
  };

  /*
  ratio: new duration / original duration
  */
  CoefficientMat reTiming(double ratio, const CoefficientMat &coeff)
  {
    CoefficientMat res_coeff;
    int order = coeff.cols();
    double t = 1.0;
    for (int curr_order = order - 1; curr_order >= 0; --curr_order)
    {
      res_coeff.col(curr_order) = coeff.col(curr_order) / t;
      t *= ratio;
    }
    return res_coeff;
  };

  void solve(const StatePVA &s, const StatePVA &e)
  {
    ROS_INFO_STREAM("[TEST]: solve s: " << s.transpose() << ", e: " << e.transpose());
    if (bvp_.solve(s, e))
    {
      CoefficientMat coeff;
      bvp_.getCoeff(coeff);
      std::cout << "ori_coeff: \n" << coeff << std::endl;
      double best_tau = bvp_.getTauStar();
      double best_cost = bvp_.getCostStar();
      ROS_INFO("[TEST]: Best cost: %lf, best tau: %lf", best_cost, best_tau);
      Piece poly_seg(best_tau, coeff);
      /* for traj vis */
      std::vector<StatePVA> vis_x;
      vis_x.clear();
      poly_seg.sampleOneSeg(&vis_x);
      vis_ptr_->visualizeStates(vis_x, BLUE, ros::Time::now());

      double vel_max = poly_seg.getMaxVelRate();
      double acc_max = poly_seg.getMaxAccRate();
      double jerk_max = poly_seg.getMaxJerkRate();
      ROS_WARN("[TEST]: vel_max: %lf, acc_max: %lf, jerk_max: %lf", vel_max, acc_max, jerk_max);

      double tau_low = best_tau / 3.0;
      double tau_high = best_tau * 3.0;
      std::vector<std::vector<StatePVA>> vis_xs;
      std::vector<std::string> texts;
      std::vector<Eigen::Vector3d> texts_pos;
      for (double t = tau_low; t <= tau_high; t += 0.1)
      {
        ros::Time t1 = ros::Time::now();
        // bvp_.calCoeffFromTau(t, coeff);
        CoefficientMat new_coeff = reTiming(t / best_tau, coeff);
        ros::Time t2 = ros::Time::now();
        Piece curr_poly_seg(t, new_coeff);
        double vel_max = curr_poly_seg.getMaxVelRate();
        ros::Time t3 = ros::Time::now();
        double acc_max = curr_poly_seg.getMaxAccRate();
        ros::Time t4 = ros::Time::now();
        double jerk_max = curr_poly_seg.getMaxJerkRate();
        ROS_WARN_STREAM("[TEST]: re-calculate coeff uses: " << (t2 - t1).toSec() * 1e6 << " us, " << (t3 - t2).toSec() * 1e6 << " us, " << (t4 - t3).toSec() * 1e6 << " us");
        std::string s = "t: " + std::to_string(t) + ", v: " + std::to_string(vel_max) + ", a: " + std::to_string(acc_max) + ", j: " + std::to_string(jerk_max);
        texts.push_back(s);
        vis_x.clear();
        curr_poly_seg.sampleOneSeg(&vis_x);
        vis_xs.push_back(vis_x);
        ROS_ERROR("[TEST]: tau: %lf, vel_max: %lf, acc_max: %lf, jerk: %lf", t, vel_max, acc_max, jerk_max);
        Eigen::Vector3d p;
        p = curr_poly_seg.getPos(t / 2.0);
        texts_pos.push_back(p);
      }
      vis_ptr_->visualizeMultiTraj(vis_xs, ros::Time::now());
      vis_ptr_->visualizeText(texts, texts_pos, ros::Time::now());
    }
    else
    {
      ROS_INFO("sth wrong with bvp_solver");
    }
  };
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "test_bap_node");
  ros::NodeHandle nh("~");
  BVPTest bvp_tester(nh);
  ros::spin();
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NQS_METROPOLISLOCALHADAMARD_HPP
#define NQS_METROPOLISLOCALHADAMARD_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include <limits>
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace nqs {

// Metropolis sampling generating local moves in hilbert space on RBM after Hadamard gate has been applied
class MetropolisLocalGate : public AbstractSampler {
  // number of visible units
  const int nv_;

  // states of visible units
  Eigen::VectorXd v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  //matrix representation of the gate
  using MatrixType = Eigen::Matrix<Complex, Eigen::Dynamic, Eigen::Dynamic>;
  MatrixType gatematrix_;

  int mynode_;
  int totalnodes_;

  // Look-up tables
  typename AbstractMachine::LookupType lt_;

  int nstates_;
  std::vector<double> localstates_;

  int sweep_size_;

 public:
  explicit MetropolisLocalGate(AbstractMachine& psi, MatrixType gate_matrix, int sweep_size)
      : AbstractSampler(psi), nv_(GetHilbert().Size()), gatematrix_(gate_matrix), sweep_size_(sweep_size) {
    Init();
  }

  void Init() {
    v_.resize(nv_);

    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    if (!GetHilbert().IsDiscrete()) {
      throw InvalidInputError(
          "Local Metropolis Gate sampler works only for discrete "
          "Hilbert spaces");
    }

    accept_.resize(1);
    moves_.resize(1);

    nstates_ = GetHilbert().LocalSize();
    localstates_ = GetHilbert().LocalStates();

    Reset(true);
  }

  void Reset(bool initrandom) override {
    if (initrandom) {
      GetHilbert().RandomVals(v_, this->GetRandomEngine());
    }

    GetMachine().InitLookup(v_, lt_);

    accept_ = Eigen::VectorXd::Zero(1);
    moves_ = Eigen::VectorXd::Zero(1);
  }

  void Sweep() override {}

  std::complex<double> PsiAfterGate(Eigen::VectorXd v, int qubit1, int qubit2) {
    if(qubit2 == -1) {
      return PsiAfter1QubitGate(v, qubit1);
    } else {
      return PsiAfter2QubitGate(v, qubit1, qubit2);
    }
  }

  std::complex<double> PsiAfter1QubitGate(Eigen::VectorXd v, int qubit) {
    double valueOfQubit = v(qubit);
    //set qubit to 0
    v(qubit) = 0.0;
    std::complex<double> psi0 = std::exp(GetMachine().LogVal(v));
    //set qubit to 1
    v(qubit) = 1.0;
    std::complex<double> psi1 = std::exp(GetMachine().LogVal(v));
    v(qubit) = valueOfQubit;

    std::complex<double> psi;

    if(valueOfQubit == 0.0) {
      psi = gatematrix_(0,0) * psi0 + gatematrix_(1,0) * psi1;
    } else {
      psi = gatematrix_(0,1) * psi0 + gatematrix_(1,1) *  psi1;
    }
    return psi;
  }

  std::complex<double> PsiAfter2QubitGate(Eigen::VectorXd v, int qubit1, int qubit2) {
    double valueOfQubit1 = v(qubit1);
    double valueOfQubit2 = v(qubit2);

    //set qubits to 00
    v(qubit1) = 0.0;
    v(qubit2) = 0.0;
    std::complex<double> psi00 = std::exp(GetMachine().LogVal(v));

    //set qubits to 01
    v(qubit1) = 0.0;
    v(qubit2) = 1.0;
    std::complex<double> psi01 = std::exp(GetMachine().LogVal(v));

    //set qubits to 10
    v(qubit1) = 1.0;
    v(qubit2) = 0.0;
    std::complex<double> psi10 = std::exp(GetMachine().LogVal(v));

    //set qubits to 11
    v(qubit1) = 1.0;
    v(qubit2) = 1.0;
    std::complex<double> psi11 = std::exp(GetMachine().LogVal(v));

    std::complex<double> psi;

    if(valueOfQubit1 == 0.0 && valueOfQubit2 == 0.0) {
      psi = gatematrix_(0,0) * psi00 + gatematrix_(1,0) * psi01 + gatematrix_(2,0) * psi10 + gatematrix_(3,0) * psi11;
    }

    if(valueOfQubit1 == 0.0 && valueOfQubit2 == 1.0) {
      psi = gatematrix_(0,1) * psi00 + gatematrix_(1,1) * psi01 + gatematrix_(2,1) * psi10 + gatematrix_(3,1) * psi11;
    }

    if(valueOfQubit1 == 1.0 && valueOfQubit2 == 0.0) {
      psi = gatematrix_(0,2) * psi00 + gatematrix_(1,2) * psi01 + gatematrix_(2,2) * psi10 + gatematrix_(3,2) * psi11;
    }

    if(valueOfQubit1 == 1.0 && valueOfQubit2 == 1.0) {
      psi = gatematrix_(0,3) * psi00 + gatematrix_(1,3) * psi01 + gatematrix_(2,3) * psi10 + gatematrix_(3,3) * psi11;
    }

    return psi;
  }

  void Sweep(int qubit1, int qubit2 = -1) {
    std::vector<int> tochange(1);
    std::vector<double> newconf(1);

    std::uniform_real_distribution<double> distu;
    std::uniform_int_distribution<int> distrs(0, nv_ - 1);
    std::uniform_int_distribution<int> diststate(0, nstates_ - 1);

    for (int i = 0; i < sweep_size_; i++) {
      // picking a random site to be changed
      int si = distrs(this->GetRandomEngine());
      assert(si < nv_);
      tochange[0] = si;

      // picking a random state
      int newstate = diststate(this->GetRandomEngine());
      newconf[0] = localstates_[newstate];

      // make sure that the new state is not equal to the current one
      while (std::abs(newconf[0] - v_(si)) <
             std::numeric_limits<double>::epsilon()) {
        newstate = diststate(this->GetRandomEngine());
        newconf[0] = localstates_[newstate];
      }

      double psiBefore = std::norm(PsiAfterGate(v_, qubit1, qubit2));
      psiBefore = psiBefore > 0 ? psiBefore : 0.00000001;

      double valueOfQubitToChange = v_(tochange[0]);
      v_(tochange[0]) = newconf[0];
      double psiAfter = std::norm(PsiAfterGate(v_, qubit1, qubit2));
      psiAfter = psiAfter > 0 ? psiAfter : 0.00000001;

      //reset
      v_(tochange[0]) = valueOfQubitToChange;

      double ratio;
      if(psiBefore != 0.0) {
        ratio = psiAfter/ psiBefore;
      } else {
        ratio = 1.0;
      }

#ifndef NDEBUG
      const auto psival1 = GetMachine().LogVal(v_);
      if (std::abs(
              std::exp(GetMachine().LogVal(v_) - GetMachine().LogVal(v_, lt_)) -
              1.) > 1.0e-8) {
        std::cerr << GetMachine().LogVal(v_) << "  and LogVal with Lt is "
                  << GetMachine().LogVal(v_, lt_) << std::endl;
        std::abort();
      }
#endif

      // Metropolis acceptance test
      if (ratio > distu(this->GetRandomEngine())) {
        accept_[0] += 1;
        GetMachine().UpdateLookup(v_, tochange, newconf, lt_);
        GetHilbert().UpdateConf(v_, tochange, newconf);

#ifndef NDEBUG
        const auto psival2 = GetMachine().LogVal(v_);
        if (std::abs(std::exp(psival2 - psival1 - lvd) - 1.) > 1.0e-8) {
          std::cerr << psival2 - psival1 << " and logvaldiff is " << lvd
                    << std::endl;
          std::cerr << psival2 << " and LogVal with Lt is "
                    << GetMachine().LogVal(v_, lt_) << std::endl;
          std::abort();
        }
#endif
      }
      moves_[0] += 1;
    }
  }

  const Eigen::VectorXd& Visible() const noexcept override { return v_; }

  void SetVisible(const Eigen::VectorXd& v) override { v_ = v; }

  AbstractMachine::VectorType DerLogVisible() override {
    return GetMachine().DerLog(v_, lt_);
  }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < 1; i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace nqs

#endif