#include "marginal_algorithm.h"

#include <Eigen/Dense>
#include <stan/math/prim/fun.hpp>

#include "lib/progressbar/progressbar.h"
#include "marginal_state.pb.h"
#include "src/collectors/base_collector.h"
#include "src/hierarchies/dependent_hierarchy.h"
#include "src/utils/eigen_utils.h"

//! \param grid Grid of points in matrix form to evaluate the density on
//! \param coll Collector containing the algorithm chain
//! \return     Matrix whose i-th column is the lpdf at i-th iteration
Eigen::MatrixXd MarginalAlgorithm::eval_lpdf(const Eigen::MatrixXd &grid,
                                             BaseCollector *coll) {
  std::deque<Eigen::VectorXd> lpdf;
  bool keep = true;
  progresscpp::ProgressBar bar(coll->get_size(), 60);

  // Loop over non-burn-in algorithm iterations
  while (keep) {
    keep = update_state_from_collector(coll);
    if (!keep) {
      break;
    }
    lpdf.push_back(lpdf_from_state(grid));
    ++bar;
    bar.display();
  }
  coll->reset();
  bar.done();
  return bayesmix::stack_vectors(lpdf);
}

Eigen::VectorXd MarginalAlgorithm::lpdf_from_state(
    const Eigen::MatrixXd &grid) {
  Eigen::VectorXd out(grid.rows());
  unsigned int n_data = curr_state.cluster_allocs_size();
  unsigned int n_clust = curr_state.cluster_states_size();
  mixing->set_state_from_proto(curr_state.mixing_state());

  // Initialize local matrix of log-densities
  Eigen::MatrixXd lpdf_local(grid.rows(), n_clust + 1);
  std::shared_ptr<BaseHierarchy> temp_hier = unique_values[0]->clone();

  Eigen::VectorXd weights(n_clust + 1);
  for (size_t j = 0; j < n_clust; j++) {
    // Extract and copy unique values in temp_hier
    temp_hier->set_state_from_proto(curr_state.cluster_states(j));
    // Compute cluster component (vector + scalar * unity vector)
    lpdf_local.col(j) =
        mixing->mass_existing_cluster(temp_hier, n_data, true, false) +
        temp_hier->like_lpdf_grid(grid).array();
  }
  // Compute marginal component (vector + scalar * unity vector)
  lpdf_local.col(n_clust) =
      mixing->mass_new_cluster(n_clust, n_data, true, false) +
      lpdf_marginal_component(temp_hier, grid).array();

  for (size_t j = 0; j < grid.rows(); j++) {
    out(j) = stan::math::log_sum_exp(lpdf_local.row(j));
  }
  return out;
}

Eigen::MatrixXd MarginalAlgorithm::eval_lpdf(const Eigen::MatrixXd &grid,
                                             const Eigen::MatrixXd &covariates,
                                             BaseCollector *coll) {
  // TODO will soon become obsolete
  std::deque<Eigen::VectorXd> lpdf;
  bool keep = true;
  progresscpp::ProgressBar bar(coll->get_size(), 60);
  while (keep) {
    keep = update_state_from_collector(coll);
    if (!keep) {
      break;
    }
    lpdf.push_back(lpdf_from_state(grid, covariates));
    ++bar;
    bar.display();
  }
  coll->reset();
  bar.done();
  return bayesmix::stack_vectors(lpdf);
}

Eigen::VectorXd MarginalAlgorithm::lpdf_from_state(
    const Eigen::MatrixXd &grid, const Eigen::MatrixXd &covariates) {
  // TODO will soon become obsolete
  Eigen::VectorXd out(grid.rows());
  unsigned int n_data = curr_state.cluster_allocs_size();
  unsigned int n_clust = curr_state.cluster_states_size();
  mixing->set_state_from_proto(curr_state.mixing_state());
  Eigen::MatrixXd lpdf_local(grid.rows(), n_clust + 1);
  auto temp_hier =
      std::dynamic_pointer_cast<DependentHierarchy>(unique_values[0]->clone());
  Eigen::VectorXd weights(n_clust + 1);
  for (size_t j = 0; j < n_clust; j++) {
    temp_hier->set_state_from_proto(curr_state.cluster_states(j));
    lpdf_local.col(j) =
        mixing->mass_existing_cluster(temp_hier, n_data, true, false) +
        temp_hier->like_lpdf_grid(grid, covariates).array();
  }
  lpdf_local.col(n_clust) =
      mixing->mass_new_cluster(n_clust, n_data, true, false) +
      lpdf_marginal_component(temp_hier, grid, covariates).array();

  for (size_t j = 0; j < grid.rows(); j++) {
    out(j) = stan::math::log_sum_exp(lpdf_local.row(j));
  }
  return out;
}

bool MarginalAlgorithm::update_state_from_collector(BaseCollector *coll) {
  bool success = coll->get_next_state(&curr_state);
  return success;
}


#include "rigid_body_assembler.hpp"

#include <Eigen/Geometry>
#include <finitediff.hpp>
#include <igl/PI.h>
#include <ipc/broad_phase/hash_grid.hpp>
#include <ipc/distance/edge_edge.hpp>
#include <tbb/parallel_sort.h>

#include <logger.hpp>
#include <physics/mass.hpp>
#include <profiler.hpp>
#include <utils/eigen_ext.hpp>
#include <utils/flatten.hpp>
#include <utils/not_implemented_error.hpp>

namespace ipc::rigid {

void RigidBodyAssembler::init(const std::vector<RigidBody>& rigid_bodies)
{
    m_rbs = rigid_bodies;

    size_t num_bodies = rigid_bodies.size();
    m_body_vertex_id.resize(num_bodies + 1);
    m_body_face_id.resize(num_bodies + 1);
    m_body_edge_id.resize(num_bodies + 1);

    // Store the starting position of each RB vertex in the global vertices
    m_body_vertex_id[0] = m_body_face_id[0] = m_body_edge_id[0] = 0;
    for (size_t i = 0; i < num_bodies; ++i) {
        auto& rb = rigid_bodies[i];
        m_body_vertex_id[i + 1] = m_body_vertex_id[i] + rb.num_vertices();
        m_body_face_id[i + 1] = m_body_face_id[i] + rb.faces.rows();
        m_body_edge_id[i + 1] = m_body_edge_id[i] + rb.edges.rows();
    }

    // global edges and faces
    m_edges.resize(m_body_edge_id.back(), 2);
    m_faces.resize(m_body_face_id.back(), 3);
    m_faces_to_edges.resize(m_body_face_id.back(), 3);
    m_codim_edges_to_edges.clear();
    for (size_t i = 0; i < num_bodies; ++i) {
        auto& rb = rigid_bodies[i];
        if (rb.edges.size() != 0) {
            m_edges.block(m_body_edge_id[i], 0, rb.edges.rows(), 2) =
                rb.edges.array() + m_body_vertex_id[i];
        }
        if (rb.faces.size() != 0) {
            m_faces.block(m_body_face_id[i], 0, rb.faces.rows(), 3) =
                rb.faces.array() + m_body_vertex_id[i];
            m_faces_to_edges.block(m_body_face_id[i], 0, rb.faces.rows(), 3) =
                rb.mesh_selector.face_to_edges().array() + m_body_edge_id[i];
        }
        for (const auto& ei : rb.mesh_selector.codim_edges_to_edges()) {
            m_codim_edges_to_edges.push_back(ei + m_body_edge_id[i]);
        }
    }
    // vertex to body map
    m_vertex_to_body_map.resize(num_vertices());
    for (size_t i = 0; i < num_bodies; ++i) {
        auto& rb = rigid_bodies[i];
        m_vertex_to_body_map.segment(m_body_vertex_id[i], rb.num_vertices())
            .setConstant(int(i));
    }
    // vertex to group id map
    m_vertex_group_ids.resize(num_vertices());
    for (size_t i = 0; i < num_bodies; ++i) {
        auto& rb = rigid_bodies[i];
        m_vertex_group_ids.segment(m_body_vertex_id[i], rb.num_vertices())
            .setConstant(rb.group_id);
    }

    // rigid body mass-matrix
    int rb_ndof = num_bodies ? rigid_bodies[0].ndof() : 0;
    m_rb_mass_matrix.resize(num_bodies * rb_ndof);
    for (int i = 0; i < int(num_bodies); ++i) {
        m_rb_mass_matrix.diagonal().segment(i * rb_ndof, rb_ndof) =
            rigid_bodies[i].mass_matrix.diagonal();
    }

    // rigid_body dof_fixed flag
    is_rb_dof_fixed.resize(num_bodies * rb_ndof);
    for (int i = 0; i < int(num_bodies); ++i) {
        auto& rb = rigid_bodies[size_t(i)];
        is_rb_dof_fixed.segment(rb_ndof * i, rb_ndof) = rb.is_dof_fixed;
    }

    // rigid_body vertex dof_fixed flag
    is_dof_fixed.resize(num_vertices(), rb_ndof);
    for (size_t i = 0; i < num_bodies; ++i) {
        auto& rb = rigid_bodies[i];
        is_dof_fixed.block(m_body_vertex_id[i], 0, rb.num_vertices(), rb_ndof) =
            rb.is_dof_fixed.transpose().replicate(rb.num_vertices(), 1);
    }

    average_edge_length = 0;
    for (const auto& body : rigid_bodies) {
        average_edge_length += body.edges.rows() * body.average_edge_length;
    }
    average_edge_length /= m_edges.rows();
    assert(std::isfinite(average_edge_length));

    average_mass = 0;
    int num_free_dof = 0;
    for (int i = 0; i < is_rb_dof_fixed.size(); i++) {
        if (!is_rb_dof_fixed[i]) {
            average_mass += m_rb_mass_matrix.diagonal()[i];
            num_free_dof++;
        }
    }
    if (num_free_dof) {
        average_mass /= num_free_dof;
    }
}

size_t RigidBodyAssembler::count_kinematic_bodies() const
{
    size_t n = 0;
    for (const auto& body : m_rbs) {
        if (body.type == RigidBodyType::KINEMATIC) {
            n++;
        }
    }
    return n;
}

void RigidBodyAssembler::global_to_local_vertex(
    const long global_vertex_id,
    long& rigid_body_id,
    long& local_vertex_id) const
{
    rigid_body_id = vertex_id_to_body_id(global_vertex_id);
    local_vertex_id =
        global_vertex_id - m_body_vertex_id[size_t(rigid_body_id)];
}

void RigidBodyAssembler::global_to_local_edge(
    const long global_edge_id, long& rigid_body_id, long& local_edge_id) const
{
    rigid_body_id = edge_id_to_body_id(global_edge_id);
    local_edge_id = global_edge_id - m_body_edge_id[size_t(rigid_body_id)];
}

void RigidBodyAssembler::global_to_local_face(
    const long global_face_id, long& rigid_body_id, long& local_face_id) const
{
    rigid_body_id = face_id_to_body_id(global_face_id);
    local_face_id = global_face_id - m_body_face_id[size_t(rigid_body_id)];
}

PosesD RigidBodyAssembler::rb_poses(const bool previous) const
{
    PosesD poses;
    poses.resize(num_bodies());
    for (size_t i = 0; i < num_bodies(); i++) {
        poses[i] = previous ? m_rbs[i].pose_prev : m_rbs[i].pose;
    }
    return poses;
}

void RigidBodyAssembler::set_rb_poses(const PosesD& poses)
{
    assert(num_bodies() == poses.size());
    for (size_t i = 0; i < num_bodies(); i++) {
        m_rbs[i].pose = poses[i];
        // if (m_rbs[i].pose.rotation.size() > 1) {
        //     // Mod 2π on the angle
        //     double angle = m_rbs[i].pose.rotation.norm();
        //     if (abs(angle) >= 2 * igl::PI) {
        //         m_rbs[i].pose.rotation /= angle;
        //         m_rbs[i].pose.rotation *= fmod(angle, 2 * igl::PI);
        //     }
        // }
    }
}

Eigen::MatrixXd
RigidBodyAssembler::world_vertices(const RigidBody::Step step) const
{
    Eigen::MatrixXd V(num_vertices(), dim());
    for (size_t i = 0; i < num_bodies(); ++i) {
        auto& rb = m_rbs[i];
        V.block(m_body_vertex_id[i], 0, rb.num_vertices(), dim()) =
            rb.world_vertices(step);
    }
    return V;
}

Eigen::MatrixXd RigidBodyAssembler::world_velocities() const
{
    Eigen::MatrixXd V(num_vertices(), dim());
    for (size_t i = 0; i < num_bodies(); ++i) {
        auto& rb = m_rbs[i];
        V.block(m_body_vertex_id[i], 0, rb.num_vertices(), dim()) =
            rb.world_velocities();
    }
    return V;
}

Eigen::MatrixXd RigidBodyAssembler::world_vertices_diff(
    const PosesD& poses,
    Eigen::MatrixXd& jac,
    Eigen::MatrixXd& hess,
    bool compute_jac,
    bool compute_hess) const
{
    assert(num_bodies() == poses.size());

    if (!compute_jac && !compute_hess) {
        return world_vertices(poses);
    }

    // We will only use auto diff to compute the derivatives of the rotation
    // matrix.
    typedef AutodiffType<Eigen::Dynamic, /*maxN=*/3> Diff;

    PROFILE_POINT("RigidBodyAssembler::world_vertices_diff");
    PROFILE_START();

    // V: Rᵐ ↦ Rⁿ (vertices flattened rowwise)
    const int n = num_vertices() * dim();
    const int m = PoseD::dim_to_ndof(dim());

    NAMED_PROFILE_POINT(
        "RigidBodyAssembler::world_vertices_diff:allocation", ALLOCATION);
    PROFILE_START(ALLOCATION);
    if (compute_jac) {
        // ∇ V(x): Rᵐ ↦ Rⁿˣᵐ
        jac.resize(n, m);
    }
    if (compute_hess) {
        // ∇²V(x): Rᵐ ↦ Rⁿˣᵐˣᵐ
        hess.resize(n * m, m); // Store as (n * m) × m matrix
    }
    Eigen::MatrixXd V(num_vertices(), dim());
    PROFILE_END(ALLOCATION);

    tbb::parallel_for(
        tbb::blocked_range<size_t>(size_t(0), num_bodies()),
        [&](const tbb::blocked_range<size_t>& range) {
            for (size_t rb_i = range.begin(); rb_i != range.end(); ++rb_i) {
                const RigidBody& rb = m_rbs[rb_i];

                // Index of rigid bodies first vertex in the global vertices
                long rb_v0_i = m_body_vertex_id[rb_i];

                if (compute_hess) {
                    rb.world_vertices_diff<Diff::DDouble2>(
                        poses[rb_i], rb_v0_i, V, jac, hess);
                } else {
                    assert(compute_jac);
                    rb.world_vertices_diff<Diff::DDouble1>(
                        poses[rb_i], rb_v0_i, V, jac, hess);
                }
            }
        });

    assert((V - world_vertices(poses)).norm() < 1e-12);

    PROFILE_END();
    return V;
}

std::vector<std::pair<int, int>> RigidBodyAssembler::close_bodies(
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const double inflation_radius) const
{
    // if (num_bodies() < 10) {
    //     return close_bodies_brute_force(
    //         poses_t0, poses_t1, inflation_radius);
    // }
    //
    // return close_bodies_hash_grid(poses_t0, poses_t1, inflation_radius);
    return close_bodies_bvh(poses_t0, poses_t1, inflation_radius);
}

std::vector<std::pair<int, int>> RigidBodyAssembler::close_bodies_brute_force(
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const double inflation_radius) const
{
    std::vector<std::pair<int, int>> close_body_pairs;
    for (int i = 0; i < num_bodies(); i++) {
        double ri = m_rbs[i].r_max;
        for (int j = i + 1; j < num_bodies(); j++) {
            if (m_rbs[i].group_id == m_rbs[j].group_id) {
                continue;
            }

            double rj = m_rbs[j].r_max;
            double distance = sqrt(edge_edge_distance(
                poses_t0[i].position, poses_t1[i].position,
                poses_t0[j].position, poses_t1[j].position));
            if (distance <= ri + rj + inflation_radius) {
                close_body_pairs.emplace_back(i, j);
            }
        }
    }
    return close_body_pairs;
}

std::vector<std::pair<int, int>> RigidBodyAssembler::close_bodies_bvh(
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const double inflation_radius) const
{
    std::vector<std::array<Eigen::Vector3d, 2>> body_bounding_boxes;
    body_bounding_boxes.reserve(num_bodies());

    NAMED_PROFILE_POINT("RigidBodyAssembler::close_bodies_bvh:build", BUILD);
    PROFILE_START(BUILD);

    for (int i = 0; i < num_bodies(); i++) {
        VectorMax3d min, max;
        m_rbs[i].compute_bounding_box(poses_t0[i], poses_t1[i], min, max);
        min.array() -= inflation_radius;
        max.array() += inflation_radius;
        Eigen::Vector3d min3D = Eigen::Vector3d::Zero(),
                        max3D = Eigen::Vector3d::Zero();
        min3D.head(dim()) = min;
        max3D.head(dim()) = max;
        body_bounding_boxes.push_back({ { min3D, max3D } });
    }

    BVH::BVH bvh;
    bvh.init(body_bounding_boxes);

    PROFILE_END(BUILD);

    NAMED_PROFILE_POINT("RigidBodyAssembler::close_bodies_bvh:query", QUERY);
    PROFILE_START(QUERY);

    std::vector<std::pair<int, int>> close_body_pairs;
    for (int i = 0; i < num_bodies(); i++) {
        std::vector<unsigned int> intersecting_body_ids;
        bvh.intersect_box(
            body_bounding_boxes[i][0], body_bounding_boxes[i][1],
            intersecting_body_ids);
        for (const auto& j : intersecting_body_ids) {
            if (i < j && m_rbs[i].group_id != m_rbs[j].group_id) {
                close_body_pairs.emplace_back(i, j);
            }
        }
    }

    PROFILE_END(QUERY);
    PROFILE_MESSAGE(
        QUERY, "num_pairs", fmt::format("{:d}", close_body_pairs.size()));

    return close_body_pairs;
}

std::vector<std::pair<int, int>> RigidBodyAssembler::close_bodies_hash_grid(
    const PosesD& poses_t0,
    const PosesD& poses_t1,
    const double inflation_radius) const
{
    HashGrid hashgrid;

    Eigen::MatrixXd V(2 * num_bodies(), dim());
    Eigen::MatrixXi E(num_bodies(), dim());
    Eigen::VectorXi group_ids(2 * num_bodies());
    double max_radius = 0;
    for (int i = 0; i < num_bodies(); i++) {
        V.row(2 * i + 0) = poses_t0[i].position;
        V.row(2 * i + 1) = poses_t1[i].position;
        E(i, 0) = 2 * i + 0;
        E(i, 1) = 2 * i + 1;
        max_radius = std::max(m_rbs[i].r_max, max_radius);
        group_ids[2 * i + 1] = group_ids[2 * i] = m_rbs[i].group_id;
    }

    // Resize the grid
    hashgrid.resize(V, V, E, max_radius + inflation_radius);

    // Add each body
    for (int i = 0; i < num_bodies(); i++) {
        hashgrid.addEdge(
            V.row(E(i, 0)), V.row(E(i, 1)), V.row(E(i, 0)), V.row(E(i, 1)), i,
            m_rbs[i].r_max);
    }

    auto can_collide = [&group_ids](size_t vi, size_t vj) {
        return group_ids[vi] != group_ids[vj];
    };

    std::vector<EdgeEdgeCandidate> body_candidates;
    hashgrid.getEdgeEdgePairs(E, body_candidates, can_collide);

    std::vector<std::pair<int, int>> close_body_pairs;
    close_body_pairs.reserve(body_candidates.size());
    for (const auto& body_candidate : body_candidates) {
        close_body_pairs.emplace_back(
            body_candidate.edge0_index, body_candidate.edge1_index);
    }

    return close_body_pairs;
}

} // namespace ipc::rigid


///////////////////////////////////////////////////////////////
//  Copyright 2015 John Maddock. 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_
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include <boost/detail/lightweight_test.hpp>
#include <boost/array.hpp>
#include "test.hpp"

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/math/constants/constants.hpp>

int main()
{
   using namespace boost::multiprecision;

   BOOST_STATIC_ASSERT((boost::is_convertible<float, cpp_bin_float_single>::value));
   BOOST_STATIC_ASSERT(!(boost::is_convertible<double, cpp_bin_float_single>::value));
   BOOST_STATIC_ASSERT(!(boost::is_convertible<long double, cpp_bin_float_single>::value));

   cpp_bin_float_single s = boost::math::constants::pi<cpp_bin_float_single>();

   typedef number<backends::cpp_bin_float<11, backends::digit_base_2, void, boost::int8_t, -14, 15>, et_off> cpp_bin_float_half;

   BOOST_STATIC_ASSERT(!(boost::is_convertible<float, cpp_bin_float_half>::value));
   BOOST_STATIC_ASSERT(!(boost::is_convertible<double, cpp_bin_float_half>::value));
   BOOST_STATIC_ASSERT(!(boost::is_convertible<long double, cpp_bin_float_half>::value));

   cpp_bin_float_half hs = boost::math::constants::pi<cpp_bin_float_half>();

   return boost::report_errors();
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_boptimizer

#include <boost/test/unit_test.hpp>

#include <limbo/limbo.hpp>

using namespace limbo;

struct Params {

    struct opt_rprop : public defaults::opt_rprop {
    };

#ifdef USE_NLOPT
    struct opt_nloptnograd : public defaults::opt_nloptnograd {
    };
#elif defined(USE_LIBCMAES)
    struct opt_cmaes : public defaults::opt_cmaes {
    };
#endif

    struct bayes_opt_bobase : public defaults::bayes_opt_bobase {
        BO_PARAM(bool, stats_enabled, false);
    };

    struct bayes_opt_boptimizer : public defaults::bayes_opt_boptimizer {
        BO_DYN_PARAM(int, hp_period);
    };

    struct stop_maxiterations {
        BO_PARAM(int, iterations, 200);
    };

    struct kernel : public defaults::kernel {
        BO_PARAM(double, noise, 1e-8);
    };

    struct kernel_exp : public defaults::kernel_exp {
        BO_PARAM(double, l, 0.2);
        BO_PARAM(double, sigma_sq, 0.25);
    };

    struct kernel_squared_exp_ard : public defaults::kernel_squared_exp_ard {
        BO_PARAM(double, sigma_sq, 0.25);
    };

    struct acqui_ucb {
        BO_PARAM(double, alpha, 1.0);
    };

    struct acqui_ei {
        BO_PARAM(double, jitter, 0.001);
    };

    struct init_randomsampling {
        BO_PARAM(int, samples, 50);
    };

    struct opt_parallelrepeater : defaults::opt_parallelrepeater {
    };
};

BO_DECLARE_DYN_PARAM(int, Params::bayes_opt_boptimizer, hp_period);

template <typename Params, int obs_size = 1>
struct eval2 {
    BO_PARAM(size_t, dim_in, 2);
    BO_PARAM(size_t, dim_out, obs_size);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        Eigen::Vector2d opt(0.25, 0.75);
        return tools::make_vector(-(x - opt).squaredNorm());
    }
};

#ifdef USE_LIBCMAES
template <typename Params, int obs_size = 1>
struct eval_unbounded {
    BO_PARAM(size_t, dim_in, 1);
    BO_PARAM(size_t, dim_out, obs_size);

    Eigen::VectorXd operator()(const Eigen::VectorXd& x) const
    {
        return tools::make_vector(-std::pow(x(0) - 2.5, 2.0));
    }
};
#endif

BOOST_AUTO_TEST_CASE(test_bo_inheritance)
{
    using namespace limbo;

    struct Parameters {
        struct stop_maxiterations {
            BO_PARAM(int, iterations, 1);
        };
    };

    Params::bayes_opt_boptimizer::set_hp_period(-1);

    using Kernel_t = kernel::Exp<Params>;
#ifdef USE_NLOPT
    using AcquiOpt_t = opt::NLOptNoGrad<Params, nlopt::GN_DIRECT_L_RAND>;
#else
    using AcquiOpt_t = opt::Cmaes<Params>;
#endif
    using Stop_t = boost::fusion::vector<stop::MaxIterations<Parameters>>;
    using Mean_t = mean::Data<Params>;
    using Stat_t = boost::fusion::vector<stat::Samples<Params>, stat::Observations<Params>>;
    using Init_t = init::NoInit<Params>;
    using GP_t = model::GP<Params, Kernel_t, Mean_t>;
    using Acqui_t = acqui::UCB<Params, GP_t>;

    bayes_opt::BOptimizer<Params, modelfun<GP_t>, initfun<Init_t>, acquifun<Acqui_t>, acquiopt<AcquiOpt_t>, statsfun<Stat_t>, stopcrit<Stop_t>> opt;
    opt.optimize(eval2<Params>());

    BOOST_CHECK(opt.total_iterations() == 1);
}

#ifdef USE_LIBCMAES
BOOST_AUTO_TEST_CASE(test_bo_unbounded)
{
    using namespace limbo;

    struct Parameters {
        struct bayes_opt_bobase : public defaults::bayes_opt_bobase {
            BO_PARAM(bool, stats_enabled, false);
            BO_PARAM(bool, bounded, false);
        };

        struct bayes_opt_boptimizer : public defaults::bayes_opt_boptimizer {
            BO_PARAM(int, hp_period, -1);
        };

        struct opt_cmaes : public defaults::opt_cmaes {
        };
    };

    using Kernel_t = kernel::Exp<Params>;
    using AcquiOpt_t = opt::Cmaes<Parameters>;
    using Stop_t = boost::fusion::vector<stop::MaxIterations<Params>>;
    using Mean_t = mean::NullFunction<Params>;
    using Stat_t = boost::fusion::vector<stat::ConsoleSummary<Params>>;
    using Init_t = init::RandomSampling<Params>;
    using GP_t = model::GP<Params, Kernel_t, Mean_t>;
    using Acqui_t = acqui::UCB<Params, GP_t>;

    bayes_opt::BOptimizer<Parameters, modelfun<GP_t>, initfun<Init_t>, acquifun<Acqui_t>, acquiopt<AcquiOpt_t>, statsfun<Stat_t>, stopcrit<Stop_t>> opt;
    opt.optimize(eval_unbounded<Params>());

    BOOST_CHECK_CLOSE(opt.best_sample()(0), 2.5, 10);
}
#endif

BOOST_AUTO_TEST_CASE(test_bo_gp)
{
    using namespace limbo;

    Params::bayes_opt_boptimizer::set_hp_period(-1);

    using Kernel_t = kernel::Exp<Params>;
#ifdef USE_NLOPT
    using AcquiOpt_t = opt::NLOptNoGrad<Params, nlopt::GN_DIRECT_L_RAND>;
#else
    using AcquiOpt_t = opt::Cmaes<Params>;
#endif
    using Stop_t = boost::fusion::vector<stop::MaxIterations<Params>>;
    using Mean_t = mean::Data<Params>;
    using Stat_t = boost::fusion::vector<stat::Samples<Params>, stat::Observations<Params>>;
    using Init_t = init::RandomSampling<Params>;
    using GP_t = model::GP<Params, Kernel_t, Mean_t>;
    using Acqui_t = acqui::EI<Params, GP_t>;

    bayes_opt::BOptimizer<Params, modelfun<GP_t>, initfun<Init_t>, acquifun<Acqui_t>, acquiopt<AcquiOpt_t>, statsfun<Stat_t>, stopcrit<Stop_t>> opt;
    opt.optimize(eval2<Params>());

    Eigen::VectorXd sol(2);
    sol << 0.25, 0.75;
    BOOST_CHECK((sol - opt.best_sample()).squaredNorm() < 1e-3);
}

BOOST_AUTO_TEST_CASE(test_bo_gp_auto)
{
    using namespace limbo;

    Params::bayes_opt_boptimizer::set_hp_period(50);

    using Kernel_t = kernel::SquaredExpARD<Params>;
#ifdef USE_NLOPT
    using AcquiOpt_t = opt::NLOptNoGrad<Params, nlopt::GN_DIRECT_L_RAND>;
#else
    using AcquiOpt_t = opt::Cmaes<Params>;
#endif
    using Stop_t = boost::fusion::vector<stop::MaxIterations<Params>>;
    using Mean_t = mean::Data<Params>;
    using Stat_t = boost::fusion::vector<stat::Samples<Params>, stat::Observations<Params>>;
    using Init_t = init::RandomSampling<Params>;
    using GP_t = model::GP<Params, Kernel_t, Mean_t, model::gp::KernelLFOpt<Params>>;
    using Acqui_t = acqui::UCB<Params, GP_t>;

    bayes_opt::BOptimizer<Params, modelfun<GP_t>, initfun<Init_t>, acquifun<Acqui_t>, acquiopt<AcquiOpt_t>, statsfun<Stat_t>, stopcrit<Stop_t>> opt;
    opt.optimize(eval2<Params>());

    Eigen::VectorXd sol(2);
    sol << 0.25, 0.75;
    BOOST_CHECK((sol - opt.best_sample()).squaredNorm() < 1e-3);
}

BOOST_AUTO_TEST_CASE(test_bo_gp_mean)
{
    using namespace limbo;

    Params::bayes_opt_boptimizer::set_hp_period(50);

    using Kernel_t = kernel::SquaredExpARD<Params>;
#ifdef USE_NLOPT
    using AcquiOpt_t = opt::NLOptNoGrad<Params, nlopt::GN_DIRECT_L_RAND>;
#else
    using AcquiOpt_t = opt::Cmaes<Params>;
#endif
    using Stop_t = boost::fusion::vector<stop::MaxIterations<Params>>;
    using Mean_t = mean::FunctionARD<Params, mean::Data<Params>>;
    using Stat_t = boost::fusion::vector<stat::Samples<Params>, stat::Observations<Params>>;
    using Init_t = init::RandomSampling<Params>;
    using GP_t = model::GP<Params, Kernel_t, Mean_t, model::gp::MeanLFOpt<Params>>;
    using Acqui_t = acqui::UCB<Params, GP_t>;

    bayes_opt::BOptimizer<Params, modelfun<GP_t>, initfun<Init_t>, acquifun<Acqui_t>, acquiopt<AcquiOpt_t>, statsfun<Stat_t>, stopcrit<Stop_t>> opt;
    opt.optimize(eval2<Params>());

    Eigen::VectorXd sol(2);
    sol << 0.25, 0.75;
    BOOST_CHECK((sol - opt.best_sample()).squaredNorm() < 1e-3);
}


#pragma once

#include <boost/range/combine.hpp>

#include "fermion_operator.hpp"
#include "observable.hpp"
#include "pauli_operator.hpp"
#include "state.hpp"
#include "type.hpp"

namespace transforms {
Observable bravyi_kitaev(FermionOperator const& fop, UINT n_qubits);
UINT count_qubits(FermionOperator const& fop);
Observable inline_sum(std::vector<Observable> summands, Observable seed);
Observable inline_product(std::vector<Observable> factors, Observable seed);
std::set<UINT> _update_set(UINT index, UINT n_qubits);
std::set<UINT> _occupation_set(UINT index);
std::set<UINT> _parity_set(UINT index);
Observable _transform_ladder_operator(
    UINT target_index, UINT action_id, UINT n_qubits);
Observable _transform_operator_term(
    SingleFermionOperator& sfop, CPPCTYPE coef, UINT n_qubits);
}  // namespace transforms


#include <iostream>
#include <boost/assign/std/vector.hpp>

#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

#include <lcm/lcm.h>

#include <ConciseArgs>

#include <pointcloud_tools/pointcloud_vis.hpp>
#include <pointcloud_tools/filter_planes.hpp>

using namespace std;
using namespace boost::assign;

int
main (int argc, char** argv)
{
  string filename= "file_name.pcd";
  ConciseArgs opt(argc, (char**)argv);
  opt.add(filename, "m", "filename",
          "filename of input pcd");
  opt.parse();
  cout << "filename: " << filename <<"\n";

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);

  // 1. Read input:
  pcl::PCDReader reader;
  reader.read (filename, *cloud_filtered);
  std::cerr << "[IN] : " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;

  int64_t fake_timestamp = 1;
  int coll_id =1;
  lcm_t * publish_lcm_;
  publish_lcm_ = lcm_create(NULL);

  pointcloud_vis* pc_vis_;
  pc_vis_ = new pointcloud_vis(publish_lcm_);

  pc_vis_->obj_cfg_list.push_back( obj_cfg(1000,"Pose - Null",5,0) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1001,"Cloud - Laser Map"     ,1,1, 1000,1, {0,0,1}));

  Isometry3dTime null_poseT = Isometry3dTime(fake_timestamp, Eigen::Isometry3d::Identity());
  pc_vis_->pose_to_lcm_from_list(1000, null_poseT);
  pc_vis_->ptcld_to_lcm_from_list(1001, *cloud_filtered, fake_timestamp, fake_timestamp);

  // 2. Extract the major planes and send them to lcm:
  FilterPlanes filtp;
  filtp.setInputCloud(cloud_filtered);
  filtp.setPoseIDs(coll_id,fake_timestamp);
  filtp.setLCM(publish_lcm_);
  vector<BasicPlane> plane_stack;
  filtp.filterPlanes(plane_stack);
  std::cout << "[OUT] number of planes extracted: " << plane_stack.size() << "\n";

  // 3. Visualise normals:
  for (size_t i = 0; i< plane_stack.size() ; i++){
    BasicPlane plane = plane_stack[i];
    char minor_char[10],major_char[10];
    sprintf(major_char,"%d",plane.major);
    sprintf(minor_char,"%d",plane.minor);
    stringstream name_out;
    name_out << "local_polygons_major_" << major_char << "_minor_" << minor_char;
    int plane_id = 290+ i ;
    ptcld_cfg pcfg = ptcld_cfg(plane_id,    name_out.str()     ,3,1, 1000,1,{-1,-1,-1} );
    pc_vis_->ptcld_to_lcm(pcfg, (plane.cloud), null_poseT.utime, null_poseT.utime );
  }

  for (size_t i=0;i<plane_stack.size();i++){
    BasicPlane plane = plane_stack[i];

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr normals_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    normals_cloud->points.push_back( plane.cloud.points[0]);
    pcl::PointXYZRGB pt;
    pt.x= plane.cloud.points[0].x + plane.coeffs.values[0];
    pt.y= plane.cloud.points[0].y + plane.coeffs.values[1];
    pt.z= plane.cloud.points[0].z + plane.coeffs.values[2];
    pt.r =0;      pt.g =255;      pt.b =0;
    normals_cloud->points.push_back( pt );

    stringstream name_out2;
    name_out2 << "global_normal_local_polygons_major_" << plane.major << "_minor_" << plane.minor;
    int plane_id2 = 590+ i  ;
    ptcld_cfg pcfg2 = ptcld_cfg(plane_id2,    name_out2.str()     ,3,1, 1000,1,{0.3,.8,0.1} );
    pc_vis_->ptcld_to_lcm(pcfg2, *normals_cloud, null_poseT.utime, null_poseT.utime );
  }

  return 0;
}


/// NOT YET IMPLEMENTED, BUT PLANNED FOR THE FUTURE...

#include <trajopt_ros/trajopt/collision_checker.hpp>
#include <boost/multi_array.hpp>
#include <cassert>

typedef boost::multi_array<float, 3> array_type;
typedef array_type::index index;

class  VoxelCollisionChecker : public CollisionChecker {
public:

  /** check everything vs everything else */
  virtual void AllVsAll(vector<Collision>& collisions)=0;
  /** check link vs everything else */
  virtual void LinkVsAll(const KinBody::Link& link, vector<Collision>& collisions)=0;
  virtual void LinksVsAll(const vector<KinBody::LinkPtr>& links, vector<Collision>& collisions)=0;

  /** contacts of distance < (arg) will be returned */
  virtual void SetContactDistance(float distance)  = 0;
  virtual double GetContactDistance() = 0;

  ~VoxelCollisionChecker() {}
  static boost::shared_ptr<CollisionChecker> GetOrCreate(OR::EnvironmentBase& env);
};

/**
Approximate each link as a union of spheres
http://www.cs.mcgill.ca/~kry/pubs/cim10/cim10.pdf
First get distance field for mesh
Find medial axis points

Trilinear interpolation

*/


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2011: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_SUCC_PRED_HPP_JOFA_080913
#define BOOST_ICL_TYPE_TRAITS_SUCC_PRED_HPP_JOFA_080913

#include <boost/icl/type_traits/is_increasing.hpp>

namespace boost{ namespace icl
{
template <class IncrementableT>
inline static IncrementableT succ(IncrementableT x) { return ++x; }

template <class DecrementableT>
inline static DecrementableT pred(DecrementableT x) { return --x; }

namespace detail
{
    template <class DomainT, bool increasing = true>
    struct successor;

    template <class DomainT>
    struct successor<DomainT, true>
    {
        typedef successor type;
        inline static DomainT apply(DomainT value){ return ++value; }
    };

    template <class DomainT>
    struct successor<DomainT, false>
    {
        typedef successor type;
        inline static DomainT apply(DomainT value){ return --value; }
    };

    template <class DomainT, bool increasing = true>
    struct predecessor;

    template <class DomainT>
    struct predecessor<DomainT, true>
    {
        typedef predecessor type;
        inline static DomainT apply(DomainT value){ return --value; }
    };

    template <class DomainT>
    struct predecessor<DomainT, false>
    {
        typedef predecessor type;
        inline static DomainT apply(DomainT value){ return ++value; }
    };
} // namespace detail

//------------------------------------------------------------------------------
template <class DomainT, class Compare>
struct successor
{
    inline static DomainT apply(DomainT value)
    {
        return detail::successor
            <DomainT, is_increasing<DomainT,Compare>::value>::apply(value);
    }
};

template <class DomainT, class Compare>
struct predecessor
{
    inline static DomainT apply(DomainT value)
    {
        return detail::predecessor
            <DomainT, is_increasing<DomainT,Compare>::value>::apply(value);
    }
};

}} // namespace boost icl

#endif


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precomp header
//
#include <mrpt/core/round.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/nav/holonomic/CHolonomicFullEval.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

#include <Eigen/Dense>  // col(),...
#include <cmath>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::nav;
using namespace std;

IMPLEMENTS_SERIALIZABLE(
	CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(
	CHolonomicFullEval, CAbstractHolonomicReactiveMethod, mrpt::nav)

const unsigned NUM_FACTORS = 7U;

CHolonomicFullEval::CHolonomicFullEval(
	const mrpt::config::CConfigFileBase* INI_FILE)
	: CAbstractHolonomicReactiveMethod("CHolonomicFullEval"),
	  m_last_selected_sector(std::numeric_limits<unsigned int>::max())
{
	if (INI_FILE != nullptr) initialize(*INI_FILE);
}

void CHolonomicFullEval::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
	options.saveToConfigFile(c, getConfigFileSectionName());
}

void CHolonomicFullEval::initialize(const mrpt::config::CConfigFileBase& c)
{
	options.loadFromConfigFile(c, getConfigFileSectionName());
}

struct TGap
{
	int k_from{-1}, k_to{-1};
	double min_eval, max_eval;
	bool contains_target_k{false};
	/** Direction with the best evaluation inside the gap. */
	int k_best_eval{-1};

	TGap()
		: min_eval(std::numeric_limits<double>::max()),
		  max_eval(-std::numeric_limits<double>::max())

	{
	}
};

void CHolonomicFullEval::evalSingleTarget(
	unsigned int target_idx, const NavInput& ni, EvalOutput& eo)
{
	ASSERT_(target_idx < ni.targets.size());
	const auto target = ni.targets[target_idx];

	using mrpt::square;

	eo = EvalOutput();

	const auto ptg = getAssociatedPTG();
	const size_t nDirs = ni.obstacles.size();

	const double target_dir = ::atan2(target.y, target.x);
	const unsigned int target_k =
		CParameterizedTrajectoryGenerator::alpha2index(target_dir, nDirs);
	const double target_dist = target.norm();

	m_dirs_scores.resize(nDirs, options.factorWeights.size() + 2);

	// TP-Obstacles in 2D:
	std::vector<mrpt::math::TPoint2D> obstacles_2d(nDirs);

	mrpt::obs::T2DScanProperties sp;
	sp.aperture = 2.0 * M_PI;
	sp.nRays = nDirs;
	sp.rightToLeft = true;
	const auto& sc_lut = m_sincos_lut.getSinCosForScan(sp);

	for (unsigned int i = 0; i < nDirs; i++)
	{
		obstacles_2d[i].x = ni.obstacles[i] * sc_lut.ccos[i];
		obstacles_2d[i].y = ni.obstacles[i] * sc_lut.csin[i];
	}

	// Sanity checks:
	ASSERT_EQUAL_(options.factorWeights.size(), NUM_FACTORS);
	ASSERT_GT_(nDirs, 3);

	for (unsigned int i = 0; i < nDirs; i++)
	{
		double scores[NUM_FACTORS];  // scores for each criterion

		// Too close to obstacles? (unless target is in between obstacles and
		// the robot)
		if (ni.obstacles[i] < options.TOO_CLOSE_OBSTACLE &&
			!(i == target_k && ni.obstacles[i] > 1.02 * target_dist))
		{
			for (size_t l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = .0;
			continue;
		}

		const double d = std::min(ni.obstacles[i], 0.95 * target_dist);

		// The TP-Space representative coordinates for this direction:
		const double x = d * sc_lut.ccos[i];
		const double y = d * sc_lut.csin[i];

		// Factor [0]: collision-free distance
		// -----------------------------------------------------
		if (mrpt::abs_diff(i, target_k) <= 1 &&
			target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
			ni.obstacles[i] > 1.05 * target_dist)
		{
			// Don't count obstacles ahead of the target.
			scores[0] =
				std::max(target_dist, ni.obstacles[i]) / (target_dist * 1.05);
		}
		else
		{
			scores[0] =
				std::max(0.0, ni.obstacles[i] - options.TOO_CLOSE_OBSTACLE);
		}

		// Discount "circular loop aparent free distance" here, but don't count
		// it for clearance, since those are not real obstacle points.
		if (ptg != nullptr)
		{
			const double max_real_freespace =
				ptg->getActualUnloopedPathLength(i);
			const double max_real_freespace_norm =
				max_real_freespace / ptg->getRefDistance();

			mrpt::keep_min(scores[0], max_real_freespace_norm);
		}

		// Factor [1]: Closest approach to target along straight line
		// (Euclidean)
		// -------------------------------------------
		mrpt::math::TSegment2D sg;
		sg.point1.x = 0;
		sg.point1.y = 0;
		sg.point2.x = x;
		sg.point2.y = y;

		// Range of attainable values: 0=passes thru target. 2=opposite
		// direction
		double min_dist_target_along_path = sg.distance(target);

		// Idea: if this segment is taking us *away* from target, don't make
		// the segment to start at (0,0), since all paths "running away"
		// will then have identical minimum distances to target. Use the
		// middle of the segment instead:
		const double endpt_dist_to_target = (target - TPoint2D(x, y)).norm();
		const double endpt_dist_to_target_norm =
			std::min(1.0, endpt_dist_to_target);

		if ((endpt_dist_to_target_norm > target_dist &&
			 endpt_dist_to_target_norm >= 0.95 * target_dist) &&
			/* the path does not get any closer to trg */
			min_dist_target_along_path >
				1.05 * std::min(target_dist, endpt_dist_to_target_norm))
		{
			// path takes us away or way blocked:
			sg.point1.x = x * 0.5;
			sg.point1.y = y * 0.5;
			min_dist_target_along_path = sg.distance(target);
		}

		scores[1] = 1.0 / (1.0 + square(min_dist_target_along_path));

		// Factor [2]: Distance of end collision-free point to target
		// (Euclidean)
		// Factor [5]: idem (except: no decimation afterwards)
		// -----------------------------------------------------
		scores[2] = std::sqrt(1.01 - endpt_dist_to_target_norm);
		scores[5] = scores[2];
		// the 1.01 instead of 1.0 is to be 100% sure we don't get a domain
		// error in sqrt()

		// Factor [3]: Stabilizing factor (hysteresis) to avoid quick switch
		// among very similar paths:
		// ------------------------------------------------------------------------------------------
		if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
		{
			// It's fine here to consider that -PI is far from +PI.
			const unsigned int hist_dist =
				mrpt::abs_diff(m_last_selected_sector, i);

			if (hist_dist >= options.HYSTERESIS_SECTOR_COUNT)
				scores[3] = square(
					1.0 - (hist_dist - options.HYSTERESIS_SECTOR_COUNT) /
							  double(nDirs));
			else
				scores[3] = 1.0;
		}
		else
		{
			scores[3] = 1.0;
		}

		// Factor [4]: clearance to nearest obstacle along path
		// Use TP-obstacles instead of real obstacles in Workspace since
		// it's way faster, despite being an approximation:
		// -------------------------------------------------------------------
		{
			sg.point1.x = 0;
			sg.point1.y = 0;
			sg.point2.x = x;
			sg.point2.y = y;

			double& closest_obs = scores[4];
			closest_obs = 1.0;

			// eval obstacles within a certain region of this "i" direction only
			const int W = std::max(1, round(nDirs * 0.1));
			const int i_min = std::max(0, static_cast<int>(i) - W);
			const int i_max =
				std::min(static_cast<int>(nDirs) - 1, static_cast<int>(i) + W);
			for (int oi = i_min; oi <= i_max; oi++)
			{
				// "no obstacle" (norm_dist=1.0) doesn't count as a real obs:
				if (ni.obstacles[oi] >= 0.99) continue;
				mrpt::keep_min(closest_obs, sg.distance(obstacles_2d[oi]));
			}
		}

		// Factor [6]: Direct distance in "sectors":
		// -------------------------------------------------------------------
		scores[6] =
			1.0 /
			(1.0 + mrpt::square((4.0 / nDirs) * mrpt::abs_diff(i, target_k)));

		// If target is not directly reachable for this i-th direction, decimate
		// its scorings:
		if (target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
			ni.obstacles[i] < 1.01 * target_dist)
		{
			// this direction cannot reach target, so assign a low score:
			scores[1] *= 0.1;
			scores[2] *= 0.1;
			scores[6] *= 0.1;
		}

		// Save stats for debugging:
		for (size_t l = 0; l < NUM_FACTORS; l++)
			m_dirs_scores(i, l) = scores[l];

	}  // end for each direction "i"

	// Normalize factors?
	ASSERT_(options.factorNormalizeOrNot.size() == NUM_FACTORS);
	for (size_t l = 0; l < NUM_FACTORS; l++)
	{
		if (!options.factorNormalizeOrNot[l]) continue;

		const double mmax = m_dirs_scores.col(l).maxCoeff();
		const double mmin = m_dirs_scores.col(l).minCoeff();
		const double span = mmax - mmin;
		if (span <= .0) continue;

		m_dirs_scores.col(l).array() -= mmin;
		m_dirs_scores.col(l).array() /= span;
	}

	// Phase 1: average of PHASE1_FACTORS and thresholding:
	// ----------------------------------------------------------------------
	const unsigned int NUM_PHASES = options.PHASE_FACTORS.size();
	ASSERT_(NUM_PHASES >= 1);

	std::vector<double> weights_sum_phase(NUM_PHASES, .0),
		weights_sum_phase_inv(NUM_PHASES);
	for (unsigned int i = 0; i < NUM_PHASES; i++)
	{
		for (unsigned int l : options.PHASE_FACTORS[i])
			weights_sum_phase[i] += options.factorWeights.at(l);
		ASSERT_(weights_sum_phase[i] > .0);
		weights_sum_phase_inv[i] = 1.0 / weights_sum_phase[i];
	}

	eo.phase_scores = std::vector<std::vector<double>>(
		NUM_PHASES, std::vector<double>(nDirs, .0));
	auto& phase_scores = eo.phase_scores;  // shortcut
	double last_phase_threshold = -1.0;  // don't threshold for the first phase

	for (unsigned int phase_idx = 0; phase_idx < NUM_PHASES; phase_idx++)
	{
		double phase_min = std::numeric_limits<double>::max(), phase_max = .0;

		for (unsigned int i = 0; i < nDirs; i++)
		{
			double this_dir_eval = 0;

			if (ni.obstacles[i] <
					options.TOO_CLOSE_OBSTACLE ||  // Too close to obstacles ?
				(phase_idx > 0 &&
				 phase_scores[phase_idx - 1][i] <
					 last_phase_threshold)  // thresholding of the previous
				// phase
			)
			{
				this_dir_eval = .0;
			}
			else
			{
				// Weighted avrg of factors:
				for (unsigned int l : options.PHASE_FACTORS[phase_idx])
					this_dir_eval +=
						options.factorWeights.at(l) *
						std::log(std::max(1e-6, m_dirs_scores(i, l)));

				this_dir_eval *= weights_sum_phase_inv[phase_idx];
				this_dir_eval = std::exp(this_dir_eval);
			}
			phase_scores[phase_idx][i] = this_dir_eval;

			mrpt::keep_max(phase_max, phase_scores[phase_idx][i]);
			mrpt::keep_min(phase_min, phase_scores[phase_idx][i]);

		}  // for each direction

		ASSERT_(options.PHASE_THRESHOLDS.size() == NUM_PHASES);
		ASSERT_(
			options.PHASE_THRESHOLDS[phase_idx] > .0 &&
			options.PHASE_THRESHOLDS[phase_idx] < 1.0);

		last_phase_threshold =
			options.PHASE_THRESHOLDS[phase_idx] * phase_max +
			(1.0 - options.PHASE_THRESHOLDS[phase_idx]) * phase_min;
	}  // end for each phase

	// Give a chance for a derived class to manipulate the final evaluations:
	auto& dirs_eval = phase_scores.back();

	postProcessDirectionEvaluations(dirs_eval, ni, target_idx);

	// Recalculate the threshold just in case the postProcess function above
	// changed things:
	{
		double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
		for (unsigned int i = 0; i < nDirs; i++)
		{
			mrpt::keep_max(phase_max, dirs_eval[i]);
			mrpt::keep_min(phase_min, dirs_eval[i]);
		}
		last_phase_threshold =
			options.PHASE_THRESHOLDS.back() * phase_max +
			(1.0 - options.PHASE_THRESHOLDS.back()) * phase_min;
	}

	// Thresholding:
	for (unsigned int i = 0; i < nDirs; i++)
	{
		double& val = dirs_eval[i];
		if (val < last_phase_threshold) val = .0;
	}
}

void CHolonomicFullEval::navigate(const NavInput& ni, NavOutput& no)
{
	using mrpt::square;

	ASSERT_(ni.clearance != nullptr);
	ASSERT_(!ni.targets.empty());

	// Create a log record for returning data.
	auto log = std::make_shared<CLogFileRecord_FullEval>();
	no.logRecord = log;

	// Evaluate for each target:
	const size_t numTrgs = ni.targets.size();
	std::vector<EvalOutput> evals(numTrgs);
	for (unsigned int trg_idx = 0; trg_idx < numTrgs; trg_idx++)
	{
		evalSingleTarget(trg_idx, ni, evals[trg_idx]);
	}

	ASSERT_(!evals.empty());
	const auto nDirs = evals.front().phase_scores.back().size();
	ASSERT_EQUAL_(nDirs, ni.obstacles.size());

	// Now, sum all weights for the last stage for each target into an "overall"
	// score vector, one score per direction of motion:
	std::vector<double> overall_scores;
	overall_scores.assign(nDirs, .0);
	for (const auto& e : evals)
	{
		for (unsigned int i = 0; i < nDirs; i++)
			overall_scores[i] += e.phase_scores.back()[i];
	}
	// Normalize:
	for (unsigned int i = 0; i < nDirs; i++)
		overall_scores[i] *= (1.0 / numTrgs);

	// Search for best direction in the "overall score" vector:

	// Keep the GAP with the largest maximum value within;
	// then pick the MIDDLE point as the final selection.
	std::vector<TGap> gaps;
	std::size_t best_gap_idx = std::string::npos;
	{
		bool inside_gap = false;
		for (unsigned int i = 0; i < nDirs; i++)
		{
			const double val = overall_scores[i];
			if (val < 0.01)
			{
				// This direction didn't pass the cut threshold for the "last
				// phase":
				if (inside_gap)
				{
					// We just ended a gap:
					auto& active_gap = *gaps.rbegin();
					active_gap.k_to = i - 1;
					inside_gap = false;
				}
			}
			else
			{
				// higher or EQUAL to the treshold (equal is important just in
				// case we have a "flat" diagram...)
				if (!inside_gap)
				{
					// We just started a gap:
					TGap new_gap;
					new_gap.k_from = i;
					gaps.emplace_back(new_gap);
					inside_gap = true;
				}
			}

			if (inside_gap)
			{
				auto& active_gap = *gaps.rbegin();
				if (val >= active_gap.max_eval)
				{
					active_gap.k_best_eval = i;
				}
				mrpt::keep_max(active_gap.max_eval, val);
				mrpt::keep_min(active_gap.min_eval, val);

				if (best_gap_idx == std::string::npos ||
					val > gaps[best_gap_idx].max_eval)
				{
					best_gap_idx = gaps.size() - 1;
				}
			}
		}  // end for i

		// Handle the case where we end with an open, active gap:
		if (inside_gap)
		{
			auto& active_gap = *gaps.rbegin();
			active_gap.k_to = nDirs - 1;
		}
	}

	// Not heading to target: go thru the "middle" of the gap to maximize
	// clearance
	int best_dir_k = -1;
	double best_dir_eval = 0;

	// We may have no gaps if all paths are blocked by obstacles, for example:
	if (!gaps.empty())
	{
		ASSERT_(best_gap_idx < gaps.size());
		const TGap& best_gap = gaps[best_gap_idx];
		best_dir_k = best_gap.k_best_eval;
		best_dir_eval = overall_scores.at(best_dir_k);
	}

	// Prepare NavigationOutput data:
	if (best_dir_eval == .0)
	{
		// No way found!
		no.desiredDirection = 0;
		no.desiredSpeed = 0;
	}
	else
	{
		// A valid movement:
		const auto ptg = getAssociatedPTG();
		const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;

		no.desiredDirection =
			CParameterizedTrajectoryGenerator::index2alpha(best_dir_k, nDirs);

		// Speed control: Reduction factors
		// ---------------------------------------------
		const double targetNearnessFactor =
			m_enableApproachTargetSlowDown
				? std::min(
					  1.0, ni.targets.front().norm() /
							   (options.TARGET_SLOW_APPROACHING_DISTANCE /
								ptg_ref_dist))
				: 1.0;

		const double obs_dist = ni.obstacles[best_dir_k];
		// Was: min with obs_clearance too.
		const double obs_dist_th = std::max(
			options.TOO_CLOSE_OBSTACLE,
			options.OBSTACLE_SLOW_DOWN_DISTANCE * ni.maxObstacleDist);
		double riskFactor = 1.0;
		if (obs_dist <= options.TOO_CLOSE_OBSTACLE)
		{
			riskFactor = 0.0;
		}
		else if (
			obs_dist < obs_dist_th && obs_dist_th > options.TOO_CLOSE_OBSTACLE)
		{
			riskFactor = (obs_dist - options.TOO_CLOSE_OBSTACLE) /
						 (obs_dist_th - options.TOO_CLOSE_OBSTACLE);
		}
		no.desiredSpeed =
			ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
	}

	m_last_selected_sector = best_dir_k;

	// LOG --------------------------
	if (log)
	{
		log->selectedTarget = 0;  // was: best_trg_idx
		log->selectedSector = best_dir_k;
		log->evaluation = best_dir_eval;
		// Copy the evaluation of first phases for (arbitrarily) the first
		// target, then overwrite the scores of its last phase with the OVERALL
		// phase scores:
		log->dirs_eval = evals.front().phase_scores;
		log->dirs_eval.back() = overall_scores;

		if (options.LOG_SCORE_MATRIX)
		{
			log->dirs_scores = m_dirs_scores;
		}
	}
}

unsigned int CHolonomicFullEval::direction2sector(
	const double a, const unsigned int N)
{
	const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
	if (idx < 0)
		return 0;
	else
		return static_cast<unsigned int>(idx);
}

CLogFileRecord_FullEval::CLogFileRecord_FullEval() : dirs_scores() {}
uint8_t CLogFileRecord_FullEval::serializeGetVersion() const { return 3; }
void CLogFileRecord_FullEval::serializeTo(
	mrpt::serialization::CArchive& out) const
{
	out << CHolonomicLogFileRecord::dirs_eval << dirs_scores << selectedSector
		<< evaluation << selectedTarget /*v3*/;
}

/*---------------------------------------------------------------
					readFromStream
  ---------------------------------------------------------------*/
void CLogFileRecord_FullEval::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		{
			if (version >= 2)
			{
				in >> CHolonomicLogFileRecord::dirs_eval;
			}
			else
			{
				CHolonomicLogFileRecord::dirs_eval.resize(2);
				in >> CHolonomicLogFileRecord::dirs_eval[0];
				if (version >= 1)
				{
					in >> CHolonomicLogFileRecord::dirs_eval[1];
				}
			}
			in >> dirs_scores >> selectedSector >> evaluation;
			if (version >= 3)
			{
				in >> selectedTarget;
			}
			else
			{
				selectedTarget = 0;
			}
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

/*---------------------------------------------------------------
						TOptions
  ---------------------------------------------------------------*/
CHolonomicFullEval::TOptions::TOptions()
	: factorWeights{0.1, 0.5, 0.5, 0.01, 1, 1, 1},
	  factorNormalizeOrNot{0, 0, 0, 0, 1, 0, 0},
	  PHASE_FACTORS{{1, 2}, {4}, {0, 2}},
	  PHASE_THRESHOLDS{0.5, 0.6, 0.7}

{
}

void CHolonomicFullEval::TOptions::loadFromConfigFile(
	const mrpt::config::CConfigFileBase& c, const std::string& s)
{
	MRPT_START

	// Load from config text:
	MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, c, s);
	MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE, double, c, s);
	MRPT_LOAD_CONFIG_VAR(OBSTACLE_SLOW_DOWN_DISTANCE, double, c, s);
	MRPT_LOAD_CONFIG_VAR(HYSTERESIS_SECTOR_COUNT, double, c, s);
	MRPT_LOAD_CONFIG_VAR(LOG_SCORE_MATRIX, bool, c, s);
	MRPT_LOAD_CONFIG_VAR(clearance_threshold_ratio, double, c, s);
	MRPT_LOAD_CONFIG_VAR(gap_width_ratio_threshold, double, c, s);

	c.read_vector(
		s, "factorWeights", std::vector<double>(), factorWeights, true);
	ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);

	c.read_vector(
		s, "factorNormalizeOrNot", factorNormalizeOrNot, factorNormalizeOrNot);
	ASSERT_EQUAL_(factorNormalizeOrNot.size(), factorWeights.size());

	// Phases:
	int PHASE_COUNT = 0;
	MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(PHASE_COUNT, int, c, s);

	PHASE_FACTORS.resize(PHASE_COUNT);
	PHASE_THRESHOLDS.resize(PHASE_COUNT);
	for (int i = 0; i < PHASE_COUNT; i++)
	{
		c.read_vector(
			s, mrpt::format("PHASE%i_FACTORS", i + 1), PHASE_FACTORS[i],
			PHASE_FACTORS[i], true);
		ASSERT_(!PHASE_FACTORS[i].empty());

		PHASE_THRESHOLDS[i] = c.read_double(
			s, mrpt::format("PHASE%i_THRESHOLD", i + 1), .0, true);
		ASSERT_(PHASE_THRESHOLDS[i] >= .0 && PHASE_THRESHOLDS[i] <= 1.0);
	}

	MRPT_END
}

void CHolonomicFullEval::TOptions::saveToConfigFile(
	mrpt::config::CConfigFileBase& c, const std::string& s) const
{
	MRPT_START

	const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
			  WV = mrpt::config::MRPT_SAVE_VALUE_PADDING();

	MRPT_SAVE_CONFIG_VAR_COMMENT(
		TOO_CLOSE_OBSTACLE,
		"Directions with collision-free distances below this threshold are not "
		"elegible.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		TARGET_SLOW_APPROACHING_DISTANCE,
		"Start to reduce speed when closer than this to target.");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		OBSTACLE_SLOW_DOWN_DISTANCE,
		"Start to reduce speed when clearance is below this value ([0,1] ratio "
		"wrt obstacle reference/max distance)");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		HYSTERESIS_SECTOR_COUNT,
		"Range of `sectors` (directions) for hysteresis over successive "
		"timesteps");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		LOG_SCORE_MATRIX, "Save the entire score matrix in log files");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		clearance_threshold_ratio,
		"Ratio [0,1], times path_count, gives the minimum number of paths at "
		"each side of a target direction to be accepted as desired direction");
	MRPT_SAVE_CONFIG_VAR_COMMENT(
		gap_width_ratio_threshold,
		"Ratio [0,1], times path_count, gives the minimum gap width to accept "
		"a direct motion towards target.");

	ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);
	c.write(
		s, "factorWeights",
		mrpt::system::sprintf_container("%.2f ", factorWeights), WN, WV,
		"[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
		"(Euclidean), [3]=Hysteresis, [4]=clearance along path, [5]=Like [2] "
		"without decimation if path obstructed");
	c.write(
		s, "factorNormalizeOrNot",
		mrpt::system::sprintf_container("%u ", factorNormalizeOrNot), WN, WV,
		"Normalize factors or not (1/0)");

	c.write(
		s, "PHASE_COUNT", PHASE_FACTORS.size(), WN, WV,
		"Number of evaluation phases to run (params for each phase below)");

	for (unsigned int i = 0; i < PHASE_FACTORS.size(); i++)
	{
		c.write(
			s, mrpt::format("PHASE%u_THRESHOLD", i + 1), PHASE_THRESHOLDS[i],
			WN, WV,
			"Phase scores must be above this relative range threshold [0,1] to "
			"be considered in next phase (Default:`0.75`)");
		c.write(
			s, mrpt::format("PHASE%u_FACTORS", i + 1),
			mrpt::system::sprintf_container("%d ", PHASE_FACTORS[i]), WN, WV,
			"Indices of the factors above to be considered in this phase");
	}

	MRPT_END
}

uint8_t CHolonomicFullEval::serializeGetVersion() const { return 4; }
void CHolonomicFullEval::serializeTo(mrpt::serialization::CArchive& out) const
{
	// Params:
	out << options.factorWeights << options.HYSTERESIS_SECTOR_COUNT
		<< options.PHASE_FACTORS <<  // v3
		options.TARGET_SLOW_APPROACHING_DISTANCE << options.TOO_CLOSE_OBSTACLE
		<< options.PHASE_THRESHOLDS  // v3
		<< options.OBSTACLE_SLOW_DOWN_DISTANCE  // v1
		<< options.factorNormalizeOrNot  // v2
		<< options.clearance_threshold_ratio
		<< options.gap_width_ratio_threshold  // v4:
		;
	// State:
	out << m_last_selected_sector;
}
void CHolonomicFullEval::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		case 1:
		case 2:
		case 3:
		case 4:
		{
			// Params:
			in >> options.factorWeights >> options.HYSTERESIS_SECTOR_COUNT;

			if (version >= 3)
			{
				in >> options.PHASE_FACTORS;
			}
			else
			{
				options.PHASE_THRESHOLDS.resize(2);
				in >> options.PHASE_FACTORS[0] >> options.PHASE_FACTORS[1];
			}
			in >> options.TARGET_SLOW_APPROACHING_DISTANCE >>
				options.TOO_CLOSE_OBSTACLE;

			if (version >= 3)
			{
				in >> options.PHASE_THRESHOLDS;
			}
			else
			{
				options.PHASE_THRESHOLDS.resize(1);
				in >> options.PHASE_THRESHOLDS[0];
			}

			if (version >= 1) in >> options.OBSTACLE_SLOW_DOWN_DISTANCE;
			if (version >= 2) in >> options.factorNormalizeOrNot;

			if (version >= 4)
			{
				in >> options.clearance_threshold_ratio >>
					options.gap_width_ratio_threshold;
			}

			// State:
			in >> m_last_selected_sector;
		}
		break;
		default:
			MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

void CHolonomicFullEval::postProcessDirectionEvaluations(
	std::vector<double>& dir_evals, const NavInput& ni, unsigned int trg_idx)
{
	// Default: do nothing
}


/*=============================================================================
    Copyright (c) 2015,2018 Kohei Takahashi

    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)
==============================================================================*/
#include <boost/config.hpp>

#ifdef BOOST_NO_CXX11_VARIADIC_TEMPLATES
#   error "does not meet requirements"
#endif

#include <boost/fusion/support/detail/index_sequence.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>

using namespace boost::fusion;

BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<0>::type, detail::index_sequence<> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<1>::type, detail::index_sequence<0> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<2>::type, detail::index_sequence<0, 1> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<3>::type, detail::index_sequence<0, 1, 2> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<4>::type, detail::index_sequence<0, 1, 2, 3> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<5>::type, detail::index_sequence<0, 1, 2, 3, 4> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<6>::type, detail::index_sequence<0, 1, 2, 3, 4, 5> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<7>::type, detail::index_sequence<0, 1, 2, 3, 4, 5, 6> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<8>::type, detail::index_sequence<0, 1, 2, 3, 4, 5, 6, 7> >));

BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<15>::type, detail::index_sequence<0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<16>::type, detail::index_sequence<0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15> >));
BOOST_MPL_ASSERT((boost::is_same<detail::make_index_sequence<17>::type, detail::index_sequence<0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16> >));


#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/gui_widgets.h>
#include <dlib/dnn.h>
#include <dlib/image_io.h>
#include <opencv2/ml/ml.hpp>

#include <iostream>
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;

using namespace dlib;
using namespace std;

std::vector<float> getDistances68(std::string left, std::string right)
{
    std::vector<float> res;

    std::vector<string> image_files;
    image_files.push_back(left);
    image_files.push_back(right);

    frontal_face_detector detector = get_frontal_face_detector();
    shape_predictor sp;
    deserialize("shape_predictor_68_face_landmarks.dat") >> sp;

//    image_window win, win_faces;
    std::vector<cv::Point2f> obj, scene;
    std::vector<cv::KeyPoint> keypoints_object, keypoints_scene;
    for (int i = 0; i < image_files.size(); ++i) //process two images
    {
        //cout << "processing image " << image_files[i] << endl;
        array2d<rgb_pixel> img;
        load_image(img, image_files[i]);

        // Now tell the face detector to give us a list of bounding boxes
        // around all the faces in the image.
        std::vector<rectangle> dets = detector(img);
        //cout << "Number of faces detected: " << dets.size() << endl;

        // Now we will go ask the shape_predictor to tell us the pose of
        // each face we detected.
        std::vector<full_object_detection> shapes;
        for (unsigned long j = 0; j < dets.size(); ++j)
        {
            full_object_detection shape = sp(img, dets[j]);
            if (i == 0) { //obj is left
                for (unsigned long s = 0; s < shape.num_parts(); ++s) {
                    cv::Point2f p = cv::Point2f(shape.part(s).x(), shape.part(s).y());
                    obj.push_back(p);
                    keypoints_object.push_back(cv::KeyPoint(p,1));
                }
            }
            else {
                for (unsigned long s = 0; s < shape.num_parts(); ++s) {
                    cv::Point2f p = cv::Point2f(shape.part(s).x(), shape.part(s).y());
                    scene.push_back(p);
                    keypoints_scene.push_back(cv::KeyPoint(p, 1));
                }
            }
            //cout << "number of parts: " << shape.num_parts() << endl;
            //cout << "pixel position of first part:  " << shape.part(0) << endl;
            //cout << "pixel position of second part: " << shape.part(1) << endl;
            // You get the idea, you can get all the face part locations if
            // you want them.  Here we just store them in shapes so we can
            // put them on the screen.
            shapes.push_back(shape);
        }

        // Now let's view our face poses on the screen.
        //win.clear_overlay();
        //win.set_image(img);
        //win.add_overlay(render_face_detections(shapes));

        // We can also extract copies of each face that are cropped, rotated upright,
        // and scaled to a standard size as shown here:
//        dlib::array<array2d<rgb_pixel> > face_chips;
//        extract_image_chips(img, get_face_chip_details(shapes), face_chips);
//        win_faces.set_image(tile_images(face_chips));
    }

    //--------------------------------------Homography(H) (begin)
    std::vector< cv::DMatch > good_matches;
    for (unsigned long i = 0; i < keypoints_object.size(); ++i) {
        good_matches.push_back(cv::DMatch(i, i, 1));
    }

    cv::Mat H = findHomography(obj, scene, CV_RANSAC);

    cv::Mat img_object = cv::imread(image_files[0], CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat img_scene = cv::imread(image_files[1], CV_LOAD_IMAGE_GRAYSCALE);

    cv::Mat img_matches;
    drawMatches(img_object, keypoints_object, img_scene, keypoints_scene,
        good_matches, img_matches, cv::Scalar::all(-1), cv::Scalar::all(-1),
        std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

//    cv::imwrite("pair.png", img_matches);

    //--------------------------------------Homography(H) (end)

    //--------------------------------------Map left points to right using H (begin)
    int num_points = 68;
    std::vector<cv::Point2f> scene__mapped_points(num_points);
    cv::perspectiveTransform(obj, scene__mapped_points, H);

    for (int i = 0; i < num_points; ++i) {
        cv::Point2f p = scene[i];
        cv::Point2f p1 = scene__mapped_points[i];
        cv::circle(img_scene, p, 1, cv::Scalar(0), 3, 8, 0); //orig black
        cv::circle(img_scene, p1, 1, cv::Scalar(255), 3, 8, 0); //mapped white
        double dist = cv::norm(p - p1);
        //cout << dist << endl;
        res.push_back(dist);
    }

//    cv::imwrite("mapped_right.png", img_scene);
    //--------------------------------------Map left points to right using H (end)

    return res;
}


/***************************************************************************************************************************
 * aruco_det.cpp
 * Author: Jario
 * Update Time: 2020.1.14
 *
 * 说明: 单个二维码识别程序，可识别的二维码在Prometheus/Modules/object_detection/config/aruco_images文件夹中
 *      视野里只允许存在一个二维码 且二维码的字典类型要对应
 *      默认二维码的边长为0.2m
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/aruco_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_aruco_det
***************************************************************************************************************************/
#include <pthread.h>
#include <thread>
#include <chrono>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <yaml-cpp/yaml.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <prometheus_msgs/ArucoInfo.h>
#include <prometheus_msgs/MultiArucoInfo.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>

#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>

#include "message_utils.h"

using namespace std;
using namespace cv;

//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【发布】检测得到的位置与姿态信息
ros::Publisher pose_pub;
ros::Publisher multi_pose_pub;
//【发布】输入检测结果图像
image_transport::Publisher aruco_pub;

//【订阅】输入开关量
ros::Subscriber switch_subscriber;
std::string switch_node_name = "/prometheus/object_detection/aruco_navigation_switch";
// 接收消息，允许暂停检测
bool is_suspanded = false;
// 使用cout打印消息
bool local_print = false;
// 使用prometheus_msgs::Message打印消息
bool message_print = true;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name = "/prometheus/message/aruco_det";

void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
    double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
    double y1 = y;
    double rz = thetaz * CV_PI / 180;
    outx = cos(rz) * x1 - sin(rz) * y1;
    outy = sin(rz) * x1 + cos(rz) * y1;
}
void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
    double x1 = x;
    double z1 = z;
    double ry = thetay * CV_PI / 180;
    outx = cos(ry) * x1 + sin(ry) * z1;
    outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
    double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确
    double z1 = z;
    double rx = thetax * CV_PI / 180;
    outy = cos(rx) * y1 - sin(rx) * z1;
    outz = cos(rx) * z1 + sin(rx) * y1;
}
// 保存上一次的检测结果
static float static_aruco_num = 0;
static float static_depth = 0;
static float static_real_x = 0;
static float static_real_y = 0;
static float static_qx = 0;
static float static_qy = 0;
static float static_qz = 0;
static float static_qw = 0;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[ArucoDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

static bool readCameraParameters(string filename, Mat &camMatrix, Mat &distCoeffs) {
    FileStorage fs(filename, FileStorage::READ);
    if(!fs.isOpened())
        return false;
    fs["camera_matrix"] >> camMatrix;
    fs["distortion_coefficients"] >> distCoeffs;
    return true;
}

// 统一消息输出函数
enum class MSG_TYPE {INFO, WARN, ERROR};
void messageEcho(std::string msg, MSG_TYPE _msg_type)
{
    if (MSG_TYPE::INFO == _msg_type)
    {
        if (local_print)
            ROS_INFO(msg.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, msg);
    }
    else if (MSG_TYPE::WARN == _msg_type)
    {
        if (local_print)
            ROS_WARN(msg.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, msg);
    }
    else if (MSG_TYPE::ERROR == _msg_type)
    {
        if (local_print)
            ROS_ERROR(msg.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, msg);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);

    // 主循环的更新频率为60HZ
    ros::Rate loop_rate(60);

    // 发布调试消息
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    // 从launch文件中读取相机参数、输入输出话题名称、aruco参数等
    std::string camera_topic = "/prometheus/camera/rgb/image_raw";
    std::string camera_params_yaml;
    std::string output_pose_topic = "/prometheus/object_detection/aruco_det";
    std::string output_multi_pose_topic = "/prometheus/object_detection/multi_aruco_det";
    std::string output_image_topic = "/prometheus/camera/rgb/image_aruco_det";
    int dictionary_id(2);
    float target_marker_length(0.0207);
    int binding_id(1);

    // 本节点读取的相机话题
    if (nh.getParam("camera_topic", camera_topic))
        messageEcho("camera_topic is: " + camera_topic, MSG_TYPE::INFO);
    else
        messageEcho("didn't find parameter camera_topic", MSG_TYPE::WARN);

    // 相机参数
    if (nh.getParam("camera_parameters", camera_params_yaml))
        messageEcho("camera_parameters is: " + camera_params_yaml, MSG_TYPE::INFO);
    else
        messageEcho("didn't find parameter camera_parameters", MSG_TYPE::WARN);

    // 发布检测结果[位姿]的话题名称-发布单个二维码
    if (nh.getParam("output_pose_topic", output_pose_topic))
        messageEcho("output_pose_topic is: " + output_pose_topic, MSG_TYPE::INFO);
    else
        messageEcho("didn't find parameter output_pose_topic", MSG_TYPE::WARN);

    // 发布检测结果[位姿]的话题名称-同时发布多个二维码
    if (nh.getParam("output_multi_pose_topic", output_multi_pose_topic))
        messageEcho("output_multi_pose_topic is: " + output_multi_pose_topic, MSG_TYPE::INFO);
    else
        messageEcho("didn't find parameter output_multi_pose_topic", MSG_TYPE::WARN);

    // 发布检测结果[检测结果图像]的话题名称
    if (nh.getParam("output_image_topic", output_image_topic))
        messageEcho("output_image_topic is: " + output_image_topic, MSG_TYPE::INFO);
    else
        messageEcho("didn't find parameter output_image_topic", MSG_TYPE::WARN);

    char temp_msg[255];
    // 二维码字典
    if (nh.getParam("dictionary_type", dictionary_id)) {
        sprintf(temp_msg, "dictionary_type is: %d", dictionary_id);
        messageEcho(temp_msg, MSG_TYPE::INFO);
    } else {
        messageEcho("didn't find parameter dictionary_type", MSG_TYPE::WARN);
    }

    // 二维码边长大小
    if (nh.getParam("target_marker_length", target_marker_length)) {
        sprintf(temp_msg, "target_marker_length is: %.3f", target_marker_length);
        messageEcho(temp_msg, MSG_TYPE::INFO);
    } else {
        messageEcho("didn't find parameter target_marker_length", MSG_TYPE::WARN);
    }

    // 绑定一个固定的二维码-检测单个二维码
    if (nh.getParam("binding_id", binding_id)) {
        sprintf(temp_msg, "binding_id is: %d", binding_id);
        messageEcho(temp_msg, MSG_TYPE::INFO);
    } else {
        messageEcho("didn't find parameter binding_id", MSG_TYPE::WARN);
    }

    //【订阅】运行状态转换开关
    switch_subscriber = nh.subscribe(switch_node_name.c_str(), 1, switchCallback);
    //【订阅】输入图像
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    //【发布】检测得到的位置与姿态信息
    pose_pub = nh.advertise<prometheus_msgs::ArucoInfo>(output_pose_topic.c_str(), 1);
    multi_pose_pub = nh.advertise<prometheus_msgs::MultiArucoInfo>(output_multi_pose_topic.c_str(), 1);
    //【发布】检测结果图像
    aruco_pub = it.advertise(output_image_topic.c_str(), 1);

    // 初始化开关量
    bool switch_state = is_suspanded;
    // 准备加载相机参数
    cv::Mat camera_matrix;
    cv::Mat distortion_coefficients;
    bool readOk = readCameraParameters(camera_params_yaml.c_str(), camera_matrix, distortion_coefficients);
    if (!readOk) {
        cerr << "Invalid camera file" << endl;
        return 0;
    }
    // 打印已加载的相机参数
    if (local_print) {
        cout << "[camera_matrix]:" << endl;
        cout << camera_matrix << endl;
        cout << "[distortion_coefficients]:" << endl;
        cout << distortion_coefficients << endl;
    }

    // ArUco Marker字典选择
    Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionary_id));
    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();

    const auto wait_duration = std::chrono::milliseconds(2000);
    // 待发布的检测结果图像
    sensor_msgs::ImagePtr results_immsg;
    // 数据帧
    cv::Mat frame;

    // 生成Aruco码
    // 保存在Prometheus/Modules/object_detection/config/aruco_images文件夹中
    /*******************************************************************************************
    cv::Mat markerImage;
    cv::Mat markerImagePad = cv::Mat::ones(500, 500,  CV_8UC1) * 255;
    for (int i=0; i<250; i++)
    {
        cv::aruco::drawMarker(dictionary, i, 400, markerImage, 1);
        markerImage.copyTo(markerImagePad.rowRange(50, 450).colRange(50, 450));
        cv::imshow("markerImagePad", markerImagePad);
        cv::waitKey(100);
        char img_fn[256];
        sprintf(img_fn, "Prometheus/Modules/object_detection/config/aruco_images/DICT_6X6_250_%d.png", i);
        cv::imwrite(img_fn, markerImagePad);
    }
    *******************************************************************************************/

    while (ros::ok())
	{
        while (!getImageStatus() && ros::ok())
        {
            messageEcho("Waiting for image.", MSG_TYPE::INFO);
            std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
                messageEcho("Start Detection.", MSG_TYPE::INFO);
            else
                messageEcho("Stop Detection.", MSG_TYPE::INFO);
        }

        if (!is_suspanded)  // 非挂起状态下才执行主函数
        {

        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            frame = cam_image_copy.clone();
        }

        if (!frame.empty())
        {
            std::vector<int> markerids;
            vector<vector<Point2f> > markerCorners, rejectedCandidate;
            Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
            cv::aruco::detectMarkers(frame, dictionary, markerCorners, markerids, parameters, rejectedCandidate);

            bool deted = false;
            if (markerids.size()>0)
            {
                vector< Vec3d > rvecs, tvecs;
                cv::aruco::estimatePoseSingleMarkers(markerCorners, target_marker_length, camera_matrix, distortion_coefficients, rvecs, tvecs);

                prometheus_msgs::MultiArucoInfo multi_pose_now;
                multi_pose_now.num_arucos = markerids.size();
                for(unsigned int i = 0; i < markerids.size(); i++)
                {
                    prometheus_msgs::ArucoInfo pose_now;
                    pose_now.header.stamp = ros::Time::now();
                    pose_now.detected = true;
                    pose_now.aruco_num = markerids[i];
                    pose_now.position[0] = tvecs[i][0];
                    pose_now.position[1] = tvecs[i][1];
                    pose_now.position[2] = tvecs[i][2];

                    // 旋转矩阵转四元数
                    cv::Mat rotation_matrix;
                    cv::Rodrigues(rvecs[i], rotation_matrix);
                    Eigen::Matrix3d rotation_matrix_eigen;
                    cv::cv2eigen(rotation_matrix, rotation_matrix_eigen);
                    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_matrix_eigen);
                    q.normalize();

                    pose_now.orientation[0] = q.x();
                    pose_now.orientation[1] = q.y();
                    pose_now.orientation[2] = q.z();
                    pose_now.orientation[3] = q.w();

                    pose_now.sight_angle[0] = atan(tvecs[i][0] / tvecs[i][2]);
                    pose_now.sight_angle[1] = atan(tvecs[i][1] / tvecs[i][2]);

                    if (binding_id == pose_now.aruco_num)
                    {
                        static_aruco_num = markerids[i];
                        static_real_x = tvecs[i][0];
                        static_real_y = tvecs[i][1];
                        static_depth  = tvecs[i][2];
                        static_qx = q.x();
                        static_qy = q.y();
                        static_qz = q.z();
                        static_qw = q.w();
                        pose_pub.publish(pose_now);

                        // 发布TF，可以在Rviz里查看结果
                        static tf::TransformBroadcaster br;
                        tf::Transform world2camera = tf::Transform(tf::Quaternion(q.x(), q.y(), q.z(), q.w()), tf::Vector3(tvecs[i][0], tvecs[i][1], tvecs[i][2]));
                        tf::StampedTransform trans_world2camera = tf::StampedTransform(world2camera, ros::Time(), "camera", "aruco");
                        br.sendTransform(trans_world2camera);

                        deted = true;
                    }

                    if (local_print) {
                        cout << "flag_detected: " << int(pose_now.detected) << endl;
                        cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<< endl;
                    }

                    multi_pose_now.aruco_infos.push_back(pose_now);
                }
                multi_pose_pub.publish(multi_pose_now);
            }
            if (!deted)
            {
                prometheus_msgs::ArucoInfo pose_now;
                pose_now.header.stamp = ros::Time::now();
                pose_now.detected = false;
                pose_now.aruco_num = static_aruco_num;
                pose_now.position[0] = static_real_x;
                pose_now.position[1] = static_real_y;
                pose_now.position[2] = static_depth;

                pose_now.orientation[0] = static_qx;
                pose_now.orientation[1] = static_qy;
                pose_now.orientation[2] = static_qz;
                pose_now.orientation[3] = static_qw;

                pose_now.sight_angle[0] = atan(static_qx / static_depth);
                pose_now.sight_angle[1] = atan(static_qy / static_depth);

                pose_pub.publish(pose_now);

                if (local_print) {
                    cout << "flag_detected: " << int(pose_now.detected) << endl;
                    cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<< endl;
                }
            }

            Mat img_copy;
            frame.copyTo(img_copy);
            cv::aruco::drawDetectedMarkers(img_copy, markerCorners, markerids);
            results_immsg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_copy).toImageMsg();
            aruco_pub.publish(results_immsg);
            cv::waitKey(1);

		}
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

	// cap.release();

}


#ifndef STAN_MATH_PRIM_MAT_FUN_DIAG_POST_MULTIPLY_HPP
#define STAN_MATH_PRIM_MAT_FUN_DIAG_POST_MULTIPLY_HPP

#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

template <typename T1, typename T2, int R1, int C1, int R2, int C2>
Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type, R1, C1>
diag_post_multiply(const Eigen::Matrix<T1, R1, C1>& m1,
                   const Eigen::Matrix<T2, R2, C2>& m2) {
  check_vector("diag_post_multiply", "m2", m2);
  check_size_match("diag_post_multiply", "m2.size()", m2.size(), "m1.cols()",
                   m1.cols());
  return m1 * m2.asDiagonal();
}

}  // namespace math
}  // namespace stan
#endif


//==============================================================================
//         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
//==============================================================================
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_SCALAR_IS_NOT_FINITE_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SCALAR_IS_NOT_FINITE_HPP_INCLUDED

#include <boost/simd/predicates/functions/is_not_finite.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/constants/false.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT( is_not_finite_, tag::cpu_
                            , (A0)
                            , (scalar_< bool_<A0> >)
                            )
  {
    typedef bool result_type;
    inline result_type operator()(A0 const&) const
    {
      return false;
    }
  };
  BOOST_DISPATCH_IMPLEMENT( is_not_finite_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    inline result_type operator()(A0 const&) const
    {
      return boost::simd::False<result_type>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT( is_not_finite_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return boost::simd::is_nan(a0-a0); }
  };
} } }

#endif


//==============================================================================
//         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
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_LEZ_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_PREDICATES_FUNCTIONS_SIMD_COMMON_IS_LEZ_HPP_INCLUDED
#include <boost/simd/toolbox/predicates/functions/is_lez.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/toolbox/predicates/functions/is_lez.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/is_less_equal.hpp>
#include <boost/simd/include/constants/zero.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::is_lez_, tag::cpu_,
                         (A0)(X),
                         ((simd_<signed_<A0>,X>))
                        )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return le(a0, Zero<A0>());
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::is_lez_, tag::cpu_,
                         (A0)(X),
                         ((simd_<unsigned_<A0>,X>))
                        )
  {
    typedef typename meta::as_logical<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return is_eqz(a0);
    }
  };
} } }
#endif


#ifndef ILP_DIGRAPH_HPP
#define ILP_DIGRAPH_HPP

#include "ilp_task.hpp"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adj_list_serialize.hpp>

#include <unordered_set>
#include <unordered_map>
#include <memory>
#include <utility>
#include <functional>

namespace ilp
{
    //////////////////////////
    //   BASE GRAPH TYPES   //
    //////////////////////////

    using VerticesStorageType = boost::vecS;
    using EdgesStorageType    = boost::vecS;
    using DirectedCategory    = boost::directedS;

    using BaseGraphTraits = boost::adjacency_list_traits<
              VerticesStorageType
            , EdgesStorageType
            , DirectedCategory
            >;

    using VertexDescriptor = BaseGraphTraits::vertex_descriptor;
    using EdgeDescriptor   = BaseGraphTraits::edge_descriptor;

    struct VertexProperty
    {
        cvector<int> point;
        VertexDescriptor predecessor = { };
        int distance = std::numeric_limits<int>::max();
    };

    struct EdgeProperty
    {
        int weight;
        int column;
    };

    using BaseGraph = boost::adjacency_list<
              VerticesStorageType
            , EdgesStorageType
            , DirectedCategory
            , VertexProperty
            , EdgeProperty
            >;

    ///////////////////////
    //   ADAPTOR GRAPH   //
    ///////////////////////

    struct DigraphAdaptor
    {
    public:
        explicit DigraphAdaptor(const ilp::ilp_task& ilpTask);

        template <typename VertexPoint>
        std::pair<VertexDescriptor, bool> add_vertex(VertexPoint&& point)
        {
            auto it = m_points_set.find(point);
            if (it != m_points_set.end())
            {
                return {it->second, false};
            }

            auto vertex_descriptor = boost::add_vertex(VertexProperty{}, m_base);
            auto [new_it, is_inserted] = m_points_set.emplace(
                    std::make_pair(std::forward<VertexPoint>(point), vertex_descriptor)
            );
            m_base[vertex_descriptor].point = new_it->first;

            return {vertex_descriptor, true};
        }

        template <typename EdgePropertyType>
        std::pair<EdgeDescriptor, bool> add_edge(VertexDescriptor lhs,
                                                 VertexDescriptor rhs,
                                                 EdgePropertyType&& property
        )
        {
            return boost::add_edge(lhs, rhs, std::forward<EdgePropertyType>(property), m_base);
        }

        bool is_feasible() const noexcept;

    public:
        const ilp::ilp_task& ilpTask;

        BaseGraph m_base;
        std::unordered_map<cvector<int>, VertexDescriptor, detail::VectorHash<int>> m_points_set;

        VertexDescriptor start;
        VertexDescriptor end;
        bool bIsFeasible;

    private:
        virtual bool populate_condition(const cvector<int>& b,
                                        const cvector<int>& p,
                                        int bound) const = 0;

        virtual void populate_from(VertexDescriptor vertex,
                                   int bound) = 0;

        virtual void populate_graph() = 0;
    };

} // namespace ilp

#endif // ILP_DIGRAPH_HPP


/*
 *  Copyright (c), 2020, EPFL - Blue Brain Project
 *
 *  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)
 *
 */
#pragma once

#include <Eigen/Eigen>

namespace HighFive {

namespace details {

//compute size for single Eigen Matrix
template <typename T, int M, int N>
inline size_t compute_total_size(const Eigen::Matrix<T,M,N>& matrix) {
    return matrix.rows() * matrix.cols();
}

//compute size for  std::vector of Eigens
template <typename T, int M, int N>
inline size_t compute_total_size(const std::vector<Eigen::Matrix<T,M,N>>& vec) {
    return std::accumulate(vec.begin(), vec.end(), size_t{0u},
        [](size_t so_far, const Eigen::Matrix<T,M,N>& v) {
            return so_far + static_cast<size_t>(v.rows()) * static_cast<size_t>(v.cols());
        });
}

#ifdef H5_USE_BOOST
// compute size for  boost::multi_array of Eigens
template <typename T, size_t Dims>
inline size_t compute_total_size(const boost::multi_array<T, Dims>& vec) {
    return std::accumulate(vec.origin(), vec.origin() + vec.num_elements(), size_t{0u},
        [](size_t so_far, const T& v) {
            return so_far + static_cast<size_t>(v.rows()) * static_cast<size_t>(v.cols());
        });
}
#endif

//compute total row size for std::vector of Eigens
template <typename T, int M, int N>
inline size_t compute_total_row_size(const std::vector<Eigen::Matrix<T,M,N>>& vec) {
    return std::accumulate(vec.begin(), vec.end(), size_t{0u},
        [](size_t so_far, const Eigen::Matrix<T,M,N>& v) {
            return so_far + static_cast<size_t>(v.rows());
        });
}

// apply conversion to eigen matrix
template <typename T, int M, int N>
struct data_converter<Eigen::Matrix<T, M, N>, void> {

    typedef Eigen::Matrix<T, M, N> MatrixTMN;

    inline data_converter(const DataSpace& space)
        : _dims(space.getDimensions()) {
        assert(_dims.size() == 2);
    }

    inline T* transform_read(MatrixTMN& array) {
        if (_dims[0] != static_cast<size_t>(array.rows()) ||
            _dims[1] != static_cast<size_t>(array.cols())) {
            array.resize(static_cast<typename MatrixTMN::Index>(_dims[0]),
                         static_cast<typename MatrixTMN::Index>(_dims[1]));
        }
        return array.data();
    }

    inline const T* transform_write(const MatrixTMN& array) {
        return array.data();
    }

    inline void process_result(MatrixTMN&) {}

    std::vector<size_t> _dims;
};

template <typename T, int M, int N>
inline void vectors_to_single_buffer(const std::vector<Eigen::Matrix<T,M,N>>& vec,
                                     const std::vector<size_t>& dims,
                                     const size_t current_dim,
                                     std::vector<T>& buffer) {

    check_dimensions_vector(compute_total_row_size(vec), dims[current_dim], current_dim);
    for (const auto& k : vec) {
        std::copy(k.data(), k.data() + k.size(), std::back_inserter(buffer));
    }
}

// apply conversion to std::vector of eigen matrix
template <typename T, int M, int N>
struct data_converter<std::vector<Eigen::Matrix<T,M,N>>, void> {

    typedef Eigen::Matrix<T, M, N> MatrixTMN;

    inline data_converter(const DataSpace& space)
        : _dims(space.getDimensions()), _space(space) {
        assert(_dims.size() == 2);
    }

    inline T * transform_read(std::vector<MatrixTMN>& /* vec */) {
        _vec_align.resize(compute_total_size(_space.getDimensions()));
        return _vec_align.data();
    }

    inline const T* transform_write(const std::vector<MatrixTMN>& vec) {
        _vec_align.reserve(compute_total_size(vec));
        vectors_to_single_buffer<T, M, N>(vec, _dims, 0, _vec_align);
        return _vec_align.data();
    }

    inline void process_result(std::vector<MatrixTMN>& vec) {
        T* start = _vec_align.data();
        if (vec.size() > 0) {
            for(auto& v : vec){
                v = Eigen::Map<MatrixTMN>(start, v.rows(), v.cols());
                start += v.rows()*v.cols();
            }
        }
        else if (M == -1 || N == -1) {
            std::ostringstream ss;
            ss << "Dynamic size(-1) used without pre-defined vector data layout.\n"
               << "Initiliaze vector elements using Zero, i.e.:\n"
               << "\t vector<MatrixXd> vec(5, MatrixXd::Zero(20,5))";
            throw DataSetException(ss.str());
        }
        else {
            for (size_t i = 0; i < _dims[0] / static_cast<size_t>(M); ++i) {
                vec.emplace_back(Eigen::Map<MatrixTMN>(start, M, N));
                start += M * N;
            }
        }
    }

    std::vector<size_t> _dims;
    std::vector<typename type_of_array<T>::type> _vec_align;
    const DataSpace& _space;
};

#ifdef H5_USE_BOOST
template <typename T, int M, int N, std::size_t Dims>
struct data_converter<boost::multi_array<Eigen::Matrix<T, M, N>, Dims>, void> {
    typedef typename boost::multi_array<Eigen::Matrix<T, M, N>, Dims> MultiArrayEigen;

    inline data_converter(const DataSpace& space)
        : _dims(space.getDimensions())
        , _space(space) {
        assert(_dims.size() == Dims);
    }

    inline T* transform_read(const MultiArrayEigen& /*array*/) {
        _vec_align.resize(compute_total_size(_space.getDimensions()));
        return _vec_align.data();
    }

    inline const T* transform_write(const MultiArrayEigen& array) {
        _vec_align.reserve(compute_total_size(array));
        for (auto e = array.origin(); e < array.origin() + array.num_elements(); ++e) {
            std::copy(e->data(), e->data() + e->size(), std::back_inserter(_vec_align));
        }
        return _vec_align.data();
    }

    inline void process_result(MultiArrayEigen& vec) {
        T* start = _vec_align.data();
        if (M != -1 && N != -1) {
            for (auto v = vec.origin(); v < vec.origin() + vec.num_elements(); ++v) {
                *v = Eigen::Map<Eigen::Matrix<T, M, N>>(start, v->rows(), v->cols());
                start += v->rows() * v->cols();
            }
        } else {
            if (vec.origin()->rows() > 0 && vec.origin()->cols() > 0) {
                const auto VEC_M = vec.origin()->rows(), VEC_N = vec.origin()->cols();
                for (auto v = vec.origin(); v < vec.origin() + vec.num_elements(); ++v) {
                    assert(v->rows() == VEC_M && v->cols() == VEC_N);
                    *v = Eigen::Map<Eigen::Matrix<T, M, N>>(start, VEC_M, VEC_N);
                    start += VEC_M * VEC_N;
                }
            } else {
                throw DataSetException(
                    "Dynamic size(-1) used without pre-defined multi_array data layout.\n"
                    "Initialize vector elements using  MatrixXd::Zero");
            }
        }
    }

    std::vector<size_t> _dims;
    const DataSpace& _space;
    std::vector<typename type_of_array<T>::type> _vec_align;
};
#endif  // H5_USE_BOOST

}  // namespace details

}  // namespace HighFive