// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <algorithm>
#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <functional>
#include <tuple>
#include <type_traits>
#include <utility>  // IWYU pragma: keep // for std::forward

#include "DataStructures/DataBox/DataBoxTag.hpp"
#include "DataStructures/DataBox/PrefixHelpers.hpp"
#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/Matrix.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Direction.hpp"
#include "Domain/ElementId.hpp"
#include "Domain/Mesh.hpp"
#include "ErrorHandling/Assert.hpp"
#include "NumericalAlgorithms/DiscontinuousGalerkin/LiftFlux.hpp"
#include "NumericalAlgorithms/LinearOperators/ApplyMatrices.hpp"
#include "NumericalAlgorithms/Spectral/Projection.hpp"
#include "Utilities/Algorithm.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TMPL.hpp"
/// \cond
template <size_t VolumeDim>
class ElementId;
template <size_t VolumeDim>
class OrientationMap;
// IWYU pragma: no_forward_declare Variables
/// \endcond

namespace dg {

template <size_t VolumeDim>
using MortarId = std::pair<::Direction<VolumeDim>, ElementId<VolumeDim>>;
template <size_t MortarDim>
using MortarSize = std::array<Spectral::MortarSize, MortarDim>;
template <size_t VolumeDim, typename ValueType>
using MortarMap = std::unordered_map<MortarId<VolumeDim>, ValueType,
                                     boost::hash<MortarId<VolumeDim>>>;

/// \ingroup DiscontinuousGalerkinGroup
/// Find a mesh for a mortar capable of representing data from either
/// of two faces.
template <size_t Dim>
Mesh<Dim> mortar_mesh(const Mesh<Dim>& face_mesh1,
                      const Mesh<Dim>& face_mesh2) noexcept;

/// \ingroup DiscontinuousGalerkinGroup
/// Determine the size of the mortar (i.e., the part of the face it
/// covers) for communicating with a neighbor.  This is the size
/// relative to the size of \p self, and will not generally agree with
/// that determined by \p neighbor.
template <size_t Dim>
MortarSize<Dim - 1> mortar_size(
    const ElementId<Dim>& self, const ElementId<Dim>& neighbor,
    size_t dimension, const OrientationMap<Dim>& orientation) noexcept;

/// \ingroup DiscontinuousGalerkinGroup
/// Determine whether data on an element face needs to be projected to a mortar.
/// If no projection is necessary the data may be used on the mortar as-is.
template <size_t Dim>
bool needs_projection(const Mesh<Dim>& face_mesh, const Mesh<Dim>& mortar_mesh,
                      const MortarSize<Dim>& mortar_size) noexcept {
  return mortar_mesh != face_mesh or
      alg::any_of(mortar_size, [](const Spectral::MortarSize& size) noexcept {
        return size != Spectral::MortarSize::Full;
      });
}

/// \ingroup DiscontinuousGalerkinGroup
/// Project variables from a face to a mortar.
template <typename Tags, size_t Dim>
Variables<Tags> project_to_mortar(const Variables<Tags>& vars,
                                  const Mesh<Dim>& face_mesh,
                                  const Mesh<Dim>& mortar_mesh,
                                  const MortarSize<Dim>& mortar_size) noexcept {
  const Matrix identity{};
  auto projection_matrices = make_array<Dim>(std::cref(identity));

  const auto face_slice_meshes = face_mesh.slices();
  const auto mortar_slice_meshes = mortar_mesh.slices();
  for (size_t i = 0; i < Dim; ++i) {
    const auto& face_slice_mesh = gsl::at(face_slice_meshes, i);
    const auto& mortar_slice_mesh = gsl::at(mortar_slice_meshes, i);
    const auto& slice_size = gsl::at(mortar_size, i);
    if (slice_size != Spectral::MortarSize::Full or
        face_slice_mesh != mortar_slice_mesh) {
      gsl::at(projection_matrices, i) = projection_matrix_element_to_mortar(
          slice_size, mortar_slice_mesh, face_slice_mesh);
    }
  }
  return apply_matrices(projection_matrices, vars, face_mesh.extents());
}

/// \ingroup DiscontinuousGalerkinGroup
/// Project variables from a mortar to a face.
template <typename Tags, size_t Dim>
Variables<Tags> project_from_mortar(
    const Variables<Tags>& vars, const Mesh<Dim>& face_mesh,
    const Mesh<Dim>& mortar_mesh, const MortarSize<Dim>& mortar_size) noexcept {
  ASSERT(face_mesh != mortar_mesh or
             alg::any_of(mortar_size,
                         [](const Spectral::MortarSize& size) noexcept {
                           return size != Spectral::MortarSize::Full;
                         }),
         "project_from_mortar should not be called if the interface mesh and "
         "mortar mesh are identical. Please elide the copy instead.");

  const Matrix identity{};
  auto projection_matrices = make_array<Dim>(std::cref(identity));

  const auto face_slice_meshes = face_mesh.slices();
  const auto mortar_slice_meshes = mortar_mesh.slices();
  for (size_t i = 0; i < Dim; ++i) {
    const auto& face_slice_mesh = gsl::at(face_slice_meshes, i);
    const auto& mortar_slice_mesh = gsl::at(mortar_slice_meshes, i);
    const auto& slice_size = gsl::at(mortar_size, i);
    if (slice_size != Spectral::MortarSize::Full or
        face_slice_mesh != mortar_slice_mesh) {
      gsl::at(projection_matrices, i) = projection_matrix_mortar_to_element(
          slice_size, face_slice_mesh, mortar_slice_mesh);
    }
  }
  return apply_matrices(projection_matrices, vars, mortar_mesh.extents());
}

namespace MortarHelpers_detail {
template <typename NormalDotNumericalFluxComputer,
          typename... NumericalFluxTags, typename... SelfTags,
          typename... PackagedTags>
void apply_normal_dot_numerical_flux(
    const gsl::not_null<Variables<tmpl::list<NumericalFluxTags...>>*>
        numerical_fluxes,
    const NormalDotNumericalFluxComputer& normal_dot_numerical_flux_computer,
    const Variables<tmpl::list<SelfTags...>>& self_packaged_data,
    const Variables<tmpl::list<PackagedTags...>>&
        neighbor_packaged_data) noexcept {
  normal_dot_numerical_flux_computer(
      make_not_null(&get<NumericalFluxTags>(*numerical_fluxes))...,
      get<PackagedTags>(self_packaged_data)...,
      get<PackagedTags>(neighbor_packaged_data)...);
}
}  // namespace MortarHelpers_detail

/// \ingroup DiscontinuousGalerkinGroup
/// Compute the lifted data resulting from computing the numerical flux.
///
/// \details
/// This applies the numerical flux, projects the result to the face
/// mesh if necessary, and then lifts it to the volume (still
/// presented only on the face mesh as all other points are zero).
///
/// Projection must happen after the numerical flux calculation so
/// that the elements on either side of the mortar calculate the same
/// result.  Projection must happen before flux lifting because we
/// want the factor of the magnitude of the unit normal added during
/// the lift to cancel the Jacobian factor in integrals to preserve
/// conservation; this only happens if the two operations are done on
/// the same grid.
template <typename FluxCommTypes, typename NormalDotNumericalFluxComputer,
          size_t Dim, typename LocalData>
auto compute_boundary_flux_contribution(
    const NormalDotNumericalFluxComputer& normal_dot_numerical_flux_computer,
    LocalData&& local_data,
    const typename FluxCommTypes::PackagedData& remote_data,
    const Mesh<Dim>& face_mesh, const Mesh<Dim>& mortar_mesh,
    const size_t extent_perpendicular_to_boundary,
    const MortarSize<Dim>& mortar_size) noexcept
    -> db::const_item_type<
        db::remove_tag_prefix<typename FluxCommTypes::normal_dot_fluxes_tag>> {
  static_assert(std::is_same_v<std::decay_t<LocalData>,
                               typename FluxCommTypes::LocalData>,
                "Second argument must be a FluxCommTypes::LocalData");
  using variables_tag =
      db::remove_tag_prefix<typename FluxCommTypes::normal_dot_fluxes_tag>;
  db::const_item_type<db::add_tag_prefix<Tags::NormalDotNumericalFlux,
                                         variables_tag>>
      normal_dot_numerical_fluxes(mortar_mesh.number_of_grid_points(), 0.0);
  MortarHelpers_detail::apply_normal_dot_numerical_flux(
      make_not_null(&normal_dot_numerical_fluxes),
      normal_dot_numerical_flux_computer, local_data.mortar_data, remote_data);

  tmpl::for_each<db::get_variables_tags_list<variables_tag>>(
      [&normal_dot_numerical_fluxes, &local_data](const auto tag) noexcept {
        using Tag = tmpl::type_from<decltype(tag)>;
        auto& numerical_flux =
            get<Tags::NormalDotNumericalFlux<Tag>>(normal_dot_numerical_fluxes);
        const auto& local_flux =
            get<Tags::NormalDotFlux<Tag>>(local_data.mortar_data);
        for (size_t i = 0; i < numerical_flux.size(); ++i) {
          numerical_flux[i] -= local_flux[i];
        }
      });

  const bool refining =
      face_mesh != mortar_mesh or
      std::any_of(mortar_size.begin(), mortar_size.end(),
                  [](const Spectral::MortarSize s) noexcept {
                    return s != Spectral::MortarSize::Full;
                  });

  return dg::lift_flux(
      refining ? project_from_mortar(normal_dot_numerical_fluxes, face_mesh,
                                     mortar_mesh, mortar_size)
               : std::move(normal_dot_numerical_fluxes),
      extent_perpendicular_to_boundary,
      std::forward<LocalData>(local_data).magnitude_of_face_normal);
}
}  // namespace dg


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <string>
#include <iostream>
#include <random>
#include <Eigen/Geometry>
#include <time.h>
#include <chrono>
#include <ros/ros.h>
#include <urdf_model/model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/robot_state.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/JointState.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <baxter_cartesian_controller/prettyprint.h>

#define _USE_MATH_DEFINES
#define BAXTER_ARM_JOINTS 7
#define TWIST_DOF 6

// Set the level of debugging verbosity
#define VERBOSE_DEBUGGING

// Set the "unit" maximum joint change/sec
#define MAXIMUM_JOINT_VELOCITY 1.0
// Set the control rate
#define CONTROL_RATE 5.0
// From the control rate, set the other core operating values
#define CONTROL_INTERVAL (1.0 / CONTROL_RATE)
#define EXECUTION_INTERVAL (2.0 * CONTROL_INTERVAL)
#define WATCHDOG_INTERVAL (3.0 * CONTROL_INTERVAL)
#define MAXIMUM_JOINT_CORRECTION (MAXIMUM_JOINT_VELOCITY * CONTROL_INTERVAL)

// Set the default PID gains
#define DEFAULT_KP 1.0
#define DEFAULT_KI 0.0
#define DEFAULT_KD 0.0

// Set damping for each of the arm joints
#define SHOULDER_0_DAMPING 0.5
#define SHOULDER_1_DAMPING 0.5
#define ELBOW_0_DAMPING 0.75
#define ELBOW_1_DAMPING 0.75
#define WRIST_0_DAMPING 1.0
#define WRIST_1_DAMPING 1.0
#define WRIST_2_DAMPING 1.0

// Set additional offsets for each of the arm joints
#define SHOULDER_0_OFFSET 0.0
#define SHOULDER_1_OFFSET 0.0
#define ELBOW_0_OFFSET 0.0
#define ELBOW_1_OFFSET 0.0
#define WRIST_0_OFFSET 0.0
#define WRIST_1_OFFSET 0.0
#define WRIST_2_OFFSET 0.0

#ifndef MOCAP_SERVOING_CONTROLLER_HPP
#define MOCAP_SERVOING_CONTROLLER_HPP

namespace baxter_mocap_servoing
{
    typedef Eigen::Affine3d Pose;
    typedef Eigen::Matrix<double, TWIST_DOF, 1> Twist;

    class MocapServoingController
    {
    protected:

        enum OPERATING_MODE {INTERNAL_POSE, EXTERNAL_POSE};
        OPERATING_MODE mode_;

        enum OPERATING_STATE {PAUSED, RUNNING};
        OPERATING_STATE state_;

        enum OPERATING_SIDE {LEFT, RIGHT};
        OPERATING_SIDE side_;

        std::vector<std::string> joint_names_;

        double max_joint_correction_;
        double execution_timestep_;

        double watchdog_timeout_;

        ros::NodeHandle nh_;

        robot_model::RobotModelPtr baxter_model_;
        robot_model::RobotStatePtr baxter_kinematic_state_;
        std::unique_ptr<robot_model::JointModelGroup> baxter_arm_group_;
        std::unique_ptr<const robot_model::LinkModel> baxter_arm_link_;
        std::unique_ptr<const robot_model::LinkModel> baxter_base_link_;

        bool arm_pose_valid_;
        bool target_pose_valid_;
        bool arm_config_valid_;
        Pose current_arm_pose_;
        Pose current_target_pose_;
        std::vector<double> current_arm_config_;

        // Storage for PID control
        Twist pose_error_integral_;
        Twist last_pose_error_;
        double kp_;
        double ki_;
        double kd_;

        ros::Subscriber arm_pose_sub_;
        ros::Subscriber target_pose_sub_;
        ros::Subscriber robot_config_sub_;

        ros::Publisher arm_controller_state_pub_;

        ros::ServiceServer abort_server_;

        std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>> arm_client_;

        ros::Timer arm_pose_watchdog_;
        ros::Timer target_pose_watchdog_;
        ros::Timer robot_config_watchdog_;

        inline void ArmPoseCB(geometry_msgs::PoseStamped arm_pose)
        {
            // First, check to make sure the frame is correct (someday, we'll use TF to make this more general)
            if (arm_pose.header.frame_id != std::string("/base") && arm_pose.header.frame_id != std::string("base"))
            {
                ROS_ERROR("Invalid frame for arm pose update - pose must be in /base frame");
            }
            // If the pose is safe, handle it
            else
            {
                // Convert to Eigen
                Eigen::Translation3d translation(arm_pose.pose.position.x, arm_pose.pose.position.y, arm_pose.pose.position.z);
                Eigen::Quaterniond rotation(arm_pose.pose.orientation.w, arm_pose.pose.orientation.x, arm_pose.pose.orientation.y, arm_pose.pose.orientation.z);
                Pose new_arm_pose = translation * rotation;
                // Set the pose
                current_arm_pose_ = new_arm_pose;
                // Set the status
                arm_pose_valid_ = true;
                // Check and set global status
                RefreshGlobalStatus();
                // Reset watchdog timer
                arm_pose_watchdog_ = nh_.createTimer(ros::Duration(watchdog_timeout_), &MocapServoingController::ArmPoseWatchdogCB, this, true);
            }
        }

        inline void ArmPoseWatchdogCB(const ros::TimerEvent& e)
        {
            ROS_WARN("Arm pose hasn't been updated in %f seconds - pausing execution until a new pose update received", watchdog_timeout_);
            arm_pose_valid_ = false;
            state_ = PAUSED;
        }

        inline void TargetPoseCB(geometry_msgs::PoseStamped target_pose)
        {
            // First, check to make sure the frame is correct (someday, we'll use TF to make this more general)
            if (target_pose.header.frame_id != std::string("/base") && target_pose.header.frame_id != std::string("base"))
            {
                ROS_ERROR("Invalid frame for target pose update - pose must be in /base frame");
            }
            // Check if the provided pose is a special "cancel target" message
            else if (target_pose.pose.orientation.x == 0.0 && target_pose.pose.orientation.y == 0.0 && target_pose.pose.orientation.z == 0.0 && target_pose.pose.orientation.w == 0.0)
            {
                ROS_INFO("Cancelling pose target, switching to PAUSED mode");
                // Set the status
                target_pose_valid_ = false;
                // Check and set the global status
                RefreshGlobalStatus();
                // We don't reset the timer, instead we cancel it
                target_pose_watchdog_.stop();
            }
            // If the pose is safe, handle it
            else
            {
                // Convert to Eigen
                Eigen::Translation3d translation(target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z);
                Eigen::Quaterniond rotation(target_pose.pose.orientation.w, target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z);
                Pose new_target_pose = translation * rotation;
                // Set the pose
                current_target_pose_ = new_target_pose;
                // Set the status
                target_pose_valid_ = true;
                // Check and set global status
                RefreshGlobalStatus();
                // Reset watchdog timer
                target_pose_watchdog_ = nh_.createTimer(ros::Duration(watchdog_timeout_), &MocapServoingController::TargetPoseWatchdogCB, this, true);
            }
        }

        inline void TargetPoseWatchdogCB(const ros::TimerEvent& e)
        {
            ROS_WARN("Target pose hasn't been updated in %f seconds - continuing to current target", watchdog_timeout_);
        }

        inline void RobotStateCB(sensor_msgs::JointState msg)
        {
            if (msg.name.size() != msg.position.size())
            {
                ROS_ERROR("Malformed configuration update - skipping update");
                return;
            }
            std::map<std::string, double> joint_positions;
            for (size_t idx = 0; idx < msg.name.size(); idx++)
            {
                joint_positions[msg.name[idx]] = msg.position[idx];
            }
            // Set the config
            std::vector<double> new_arm_config(BAXTER_ARM_JOINTS);
            new_arm_config[0] = joint_positions[joint_names_[0]];
            new_arm_config[1] = joint_positions[joint_names_[1]];
            new_arm_config[2] = joint_positions[joint_names_[2]];
            new_arm_config[3] = joint_positions[joint_names_[3]];
            new_arm_config[4] = joint_positions[joint_names_[4]];
            new_arm_config[5] = joint_positions[joint_names_[5]];
            new_arm_config[6] = joint_positions[joint_names_[6]];
            current_arm_config_ = new_arm_config;
            // If we're in INTERNAL_POSE mode, compute the arm pose
            if (mode_ == INTERNAL_POSE)
            {
                current_arm_pose_ = ComputeArmPose(current_arm_config_);
            }
            // Set the status
            arm_config_valid_ = true;
            // Check and set global status
            RefreshGlobalStatus();
            // Reset watchdog timer
            robot_config_watchdog_ = nh_.createTimer(ros::Duration(watchdog_timeout_), &MocapServoingController::RobotConfigWatchdogCB, this, true);
        }

        inline void RobotConfigWatchdogCB(const ros::TimerEvent& e)
        {
            ROS_WARN("Robot config hasn't been updated in %f seconds - pausing execution until a new config update received", watchdog_timeout_);
            arm_config_valid_ = false;
            state_ = PAUSED;
        }

        inline bool AbortCB(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
        {
            // Cancel the current pose target
            ROS_INFO("Cancelling pose target, switching to PAUSED mode");
            // Set the status
            target_pose_valid_ = false;
            // Check and set the global status
            RefreshGlobalStatus();
            // We don't reset the timer, instead we cancel it
            target_pose_watchdog_.stop();
            return true;
        }

        inline Pose ComputeArmPose(std::vector<double>& current_configuration)
        {
            // Update joint values
            baxter_kinematic_state_->setJointGroupPositions(baxter_arm_group_.get(), current_configuration);
            // Update the joint transforms
            baxter_kinematic_state_->update(true);
            // Get the transform from base to torso
            Pose current_base_pose = baxter_kinematic_state_->getGlobalLinkTransform(baxter_base_link_.get());
            // Get the transform from base to wrist
            Pose current_base_to_wrist_pose = baxter_kinematic_state_->getGlobalLinkTransform(baxter_arm_link_.get());
            // Get the arm pose
            Pose current_arm_pose = current_base_pose.inverse() * current_base_to_wrist_pose;
            return current_arm_pose;
        }

        inline Eigen::MatrixXd ComputeJacobian(std::vector<double>& current_configuration)
        {
            // Update joint values
            baxter_kinematic_state_->setJointGroupPositions(baxter_arm_group_.get(), current_configuration);
            // Update the joint transforms
            baxter_kinematic_state_->update(true);
            // Compute the Jacobian
            Eigen::MatrixXd current_jacobian = baxter_kinematic_state_->getJacobian(baxter_arm_group_.get());
            return current_jacobian;
        }

        inline Twist ComputePoseError(Pose& arm_pose, Pose& target_pose)
        {
            std::cout << "Current arm pose:\n" << arm_pose.translation() << std::endl;
            std::cout << "Current target pose:\n" << target_pose.translation() << std::endl;
            Twist pose_error;
            pose_error.head<3>() = arm_pose.translation() - target_pose.translation();
            pose_error.tail<3>() = 0.5 * (target_pose.linear().col(0).cross(arm_pose.linear().col(0)) + target_pose.linear().col(1).cross(arm_pose.linear().col(1)) + target_pose.linear().col(2).cross(arm_pose.linear().col(2)));
            pose_error = -1.0 * pose_error;
            std::cout << "Computed pose error:\n" << pose_error << std::endl;
            return pose_error;
        }

        inline void RefreshGlobalStatus()
        {
            if (mode_ == INTERNAL_POSE)
            {
                if (target_pose_valid_ && arm_config_valid_)
                {
                    state_ = RUNNING;
                }
                else
                {
                    state_ = PAUSED;
                }
            }
            else
            {
                if (arm_pose_valid_ && target_pose_valid_ && arm_config_valid_)
                {
                    state_ = RUNNING;
                }
                else
                {
                    state_ = PAUSED;
                }
            }
        }

        std::vector<double> ComputeNextStep(Pose& current_arm_pose, Pose& current_target_pose, std::vector<double>& current_configuration);

        void CommandToTarget(std::vector<double>& current_config, std::vector<double>& target_config);

    public:

        MocapServoingController(ros::NodeHandle& nh, std::string group_name, std::string arm_pose_topic, std::string target_pose_topic, std::string robot_config_topic, std::string arm_command_action, std::string arm_controller_state_topic, std::string abort_service, double kp, double ki, double kd);

        MocapServoingController(ros::NodeHandle &nh, std::string group_name, std::string target_pose_topic, std::string robot_config_topic, std::string arm_command_action, std::string arm_controller_state_topic, std::string abort_service, double kp, double ki, double kd);

        ~MocapServoingController() {}

        void Loop();
    };
}

#endif // MOCAP_SERVOING_CONTROLLER_HPP


#ifndef THYME_ENVIRONMENTS_ROVERS_ENVIRONMENT
#define THYME_ENVIRONMENTS_ROVERS_ENVIRONMENT

#include <Eigen/Dense>
#include <rovers/core/poi/count_constraint.hpp>
#include <rovers/core/poi/poi.hpp>
#include <rovers/core/poi/type_constraint.hpp>
#include <rovers/core/rover/rover.hpp>
#include <rovers/core/sensors/lidar.hpp>
#include <rovers/core/setup/init_random.hpp>
#include <rovers/utilities/spaces/discrete.hpp>
#include <tuple>
#include <vector>

namespace rovers {

/*
 *
 * Default Rovers environment
 *
 */
template <typename InitPolicy = RandomInit>
class Environment {
   public:
    using Action = Eigen::MatrixXd;
    using State = std::vector<Eigen::MatrixXd>;
    using Reward = std::vector<double>;

    Environment(InitPolicy initPolicy = InitPolicy(), std::vector<Agent> rovers = {},
                std::vector<Entity> pois = {}, size_t width = 10.0, size_t height = 10.0)
        : m_initPolicy(initPolicy),
          m_rovers(std::move(rovers)),
          m_pois(std::move(pois)),
          m_width(width),
          m_height(height) {}

    // helpers to set rovers/pois after the fact
    void set_rovers(std::vector<Agent> rovers) { m_rovers = std::move(rovers); }
    void set_pois(std::vector<Entity> pois) { m_pois = std::move(pois); }

    const std::vector<Agent>& rovers() { return m_rovers; }
    const std::vector<Entity>& pois() { return m_pois; }

    std::tuple<State, Reward> step(std::vector<Action> actions) {
        for (size_t i = 0; i < m_rovers.size(); ++i) {
            auto& rover = m_rovers[i];
            // call update for all rovers
            rover->update();
            // take actions
            rover->act(actions[i]);
            // bound position
            clamp_bounds(rover);
        }
        // call update for pois
        for (auto& poi : m_pois) poi->update();
        // return next observations and rewards
        return status();
    }

    std::tuple<State, Reward> reset() {
        // clear agents
        for (auto& r : m_rovers) r->reset();
        // reset pois
        for (auto& poi : m_pois) poi->set_observed(false);
        // initialize
        m_initPolicy.initialize(m_rovers, m_pois);
        // return next observations and rewards
        return status();
    }

    void render() {}
    void close() {}

    const size_t& width() { return m_width; }
    const size_t& height() { return m_height; }
    // TODO add pre/post update for all components

   private:
    inline void clamp_bounds(Agent& rover) {
        rover->set_position(std::clamp(rover->position().x, 0.0, 1.0 * m_width),
                            std::clamp(rover->position().y, 0.0, 1.0 * m_height));
    }

    std::tuple<State, Reward> status() {
        // observations and rewards
        State state;
        Reward rewards;
        for (auto& r : m_rovers) {
            state.push_back(r->scan({r, m_rovers, m_pois}));
            rewards.push_back(r->reward({r, m_rovers, m_pois}));
        }
        return {state, rewards};
    }

   private:
    InitPolicy m_initPolicy;
    std::vector<Agent> m_rovers;
    std::vector<Entity> m_pois;

    size_t m_width;
    size_t m_height;
};

/*
 *
 * Syntactic sugar for agents/entities
 *
 */
using Agents = std::vector<Agent>;
using Entities = std::vector<Entity>;
using Actions = std::vector<Eigen::MatrixXd>;

Eigen::MatrixXd tensor(std::vector<double> list) {
    return Eigen::Map<Eigen::MatrixXd>(list.data(), list.size(), 1);
}

Agents& operator<<(Agents& vector, Agent&& rover) {
    vector.push_back(std::move(rover));
    return vector;
}
Entities& operator<<(Entities& vector, Entity&& poi) {
    vector.push_back(std::move(poi));
    return vector;
}

}  // namespace rovers

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          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_SWAR_FUNCTIONS_SIMD_SSE_SSE2_ALL_REDUCE_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_ALL_REDUCE_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/swar/functions/all_reduce.hpp>
#include <boost/simd/sdk/simd/category.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( all_reduce_
                                    , boost::simd::tag::sse2_
                                    , (Tag)(A0)
                                    , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                                      ((target_< unspecified_<Tag> >))
                                    )
  {
    typedef typename Tag::type                              base;
    typedef typename dispatch::meta::call<base( A0 const&
                                              , A0 const&
                                              )>::type      result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, Tag const&) const
    {
      // Buttefly reduction optimized for int16
      dispatch::functor<base> op;

      A0 a1 = _mm_shuffle_epi32(a0, _MM_SHUFFLE(1, 0, 3, 2));
      result_type t1 = op(a0,a1);
      result_type t2 = _mm_shuffle_epi32(t1, _MM_SHUFFLE(2, 3, 0, 1));
                  t2 = op(t2, t1);

      t1 = _mm_shufflehi_epi16(t2, _MM_SHUFFLE(0, 1, 2, 3));
      t1 = _mm_shufflelo_epi16(t1, _MM_SHUFFLE(0, 1, 2, 3));

      return op(t2, t1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( all_reduce_
                                    , boost::simd::tag::sse2_
                                    , (Tag)(A0)
                                    , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                                      ((target_< unspecified_<Tag> >))
                                    )
  {
    typedef typename Tag::type                              base;
    typedef typename dispatch::meta::call<base( A0 const&
                                              , A0 const&
                                              )>::type      result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, Tag const&) const
    {
      // Buttefly reduction optimized for int8
      dispatch::functor<base> op;

      // 16 -> 2x8
      A0          a1 = _mm_shuffle_epi32(a0, _MM_SHUFFLE(1, 0, 3, 2));
      result_type t1 = op(a0,a1);

      // 2x8 -> 4x4
      result_type t2 = _mm_shuffle_epi32(t1, _MM_SHUFFLE(2, 3, 0, 1));
                  t2 = op(t2, t1);

      // 4x4 -> 8x2
      t1 = _mm_shufflehi_epi16(t2, _MM_SHUFFLE(0, 1, 2, 3));
      t1 = _mm_shufflelo_epi16(t1, _MM_SHUFFLE(0, 1, 2, 3));
      t1 = op(t2,t1);

      // Reorder [ a b ... a b ] into [ a a b b ... a a b b]
      t1 = _mm_unpacklo_epi8(t1,t1);
      t1 = _mm_unpackhi_epi8(t1,t1);

      // Turn [ a a b b ... a a b b] into [ b b a a ... b b a a]
      t2 = _mm_shufflehi_epi16(t1, _MM_SHUFFLE(0, 1, 2, 3));
      t2 = _mm_shufflelo_epi16(t2, _MM_SHUFFLE(0, 1, 2, 3));

      return op(t2, t1);
    }
  };
} } }

#endif

#endif


#include <scorum/chain/advertising/advertising_auction.hpp>

#include <scorum/chain/schema/budget_objects.hpp>

#include <boost/range/algorithm/reverse.hpp>
#include <boost/range/algorithm/transform.hpp>

#include <scorum/chain/services/dynamic_global_property.hpp>
#include <scorum/chain/services/advertising_property.hpp>
#include <scorum/chain/services/budgets.hpp>

#include <scorum/utils/take_n_range.hpp>
#include <scorum/utils/collect_range_adaptor.hpp>

namespace scorum {
namespace chain {

advertising_auction::advertising_auction(dynamic_global_property_service_i& dprops_svc,
                                         advertising_property_service_i& adv_props_svc,
                                         adv_budget_service_i<budget_type::post>& post_budget_svc,
                                         adv_budget_service_i<budget_type::banner>& banner_budget_svc)
    : _dprops_svc(dprops_svc)
    , _adv_props_svc(adv_props_svc)
    , _post_budget_svc(post_budget_svc)
    , _banner_budget_svc(banner_budget_svc)
{
}

void advertising_auction::run_round()
{
    const auto& post_coeffs = _adv_props_svc.get().auction_post_coefficients;
    const auto& banner_coeffs = _adv_props_svc.get().auction_banner_coefficients;

    run_round(_post_budget_svc, std::vector<percent_type>{ post_coeffs.begin(), post_coeffs.end() });
    run_round(_banner_budget_svc, std::vector<percent_type>{ banner_coeffs.begin(), banner_coeffs.end() });
}

template <budget_type budget_type_v>
void advertising_auction::run_round(adv_budget_service_i<budget_type_v>& budget_svc,
                                    const std::vector<percent_type>& coeffs)
{
    namespace br = boost::range;

    auto budgets = budget_svc.get_top_budgets(_dprops_svc.head_block_time());
    std::vector<asset> per_block_list;
    br::transform(budgets, std::back_inserter(per_block_list), [](auto b) { return b.get().per_block; });

    auto valuable_per_block_vec = boost::make_iterator_range(per_block_list)
        | utils::adaptors::take_n(coeffs.size() + 1) //
        | utils::adaptors::collect<std::vector>();

    auto auction_bets = calculate_bets(valuable_per_block_vec, coeffs);

    for (size_t i = 0; i < budgets.size(); ++i)
    {
        const auto& budget = budgets[i].get();

        auto per_block = budget_svc.allocate_cash(budget);

        auto adv_cash = asset(0, per_block.symbol());
        if (i < auction_bets.size())
            adv_cash = auction_bets[i];

        auto ret_cash = per_block - adv_cash;
        FC_ASSERT(ret_cash.amount >= 0, "Owner's cash-back amount cannot be less than zero");

        budget_svc.update_pending_payouts(budget, ret_cash, adv_cash);
    }
}

std::vector<asset> advertising_auction::calculate_bets(const std::vector<asset>& per_blocks,
                                                       const std::vector<percent_type>& coeffs)
{
    FC_ASSERT(coeffs.size() > 0, "invalid coefficient's list");
    FC_ASSERT(per_blocks.size() <= coeffs.size() + 1, "invalid list of per-block values");
    FC_ASSERT(std::is_sorted(coeffs.rbegin(), coeffs.rend()), "per-block list isn't sorted");
    FC_ASSERT(std::is_sorted(per_blocks.rbegin(), per_blocks.rend()), "Auction coefficients aren't sorted");
    FC_ASSERT(*coeffs.rbegin() > 0, "Auction coefficients should be positive");
    FC_ASSERT(*coeffs.begin() <= 100, "Auction coefficients should be less than 100");
    FC_ASSERT(per_blocks.empty() || per_blocks.rbegin()->amount > 0, "per-block amount should be positive");

    if (per_blocks.empty())
        return {};

    int64_t bets_size = std::min(per_blocks.size(), coeffs.size());
    std::vector<asset> bets;

    auto smallest_bet = bets_size < (int64_t)per_blocks.size() //
        ? per_blocks[bets_size]
        : per_blocks[bets_size - 1];
    bets.push_back(smallest_bet);

    for (int64_t ridx = bets_size - 2; ridx >= 0l; --ridx)
    {
        auto factor = utils::make_fraction((coeffs[ridx] - coeffs[ridx + 1]), coeffs.front());
        auto bet = bets.back() + per_blocks[ridx + 1] * factor;
        bets.push_back(std::min(bet, per_blocks[ridx]));
    }

    boost::reverse(bets);

    return bets;
}
}
}


#ifndef PYTHONIC_INCLUDE_NUMPY_SIN_HPP
#define PYTHONIC_INCLUDE_NUMPY_SIN_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"
#include <boost/simd/function/sin.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME sin
#define NUMPY_NARY_FUNC_SYM boost::simd::sin
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


/************************************************************************
 *
 *
 *  Copyright 2015  Arun Das (University of Waterloo)
 *                      [adas@uwaterloo.ca]
 *                  James Servos (University of Waterloo)
 *                      [jdservos@uwaterloo.ca]
 *
 *
 *************************************************************************/
#include <Eigen/Eigenvalues>
#include <wave/matching/groundSegmentation.hpp>
#include <wave/matching/PointcloudXYZGD.hpp>

namespace wave {

bool compareSignalPoints(const signalPoint &a, const signalPoint &b) {
    return a.height < b.height;
}

groundSegmentation::groundSegmentation(GroundSegmentationParams config) {
    this->params = config;
    this->pBG = new polarBinGrid;
    initializePolarBinGrid();
}

void groundSegmentation::initializePolarBinGrid(void) {
    this->pBG->aCell.resize(this->params.num_bins_a);
    for (int i = 0; i < this->params.num_bins_a; i++) {
        this->pBG->aCell[i].sigPoints.clear();
        this->pBG->aCell[i].lCell.resize(this->params.num_bins_l);
        this->pBG->aCell[i].sigPoints.resize(this->params.num_bins_l);
        this->pBG->aCell[i].rangeHeightSignal.resize(this->params.num_bins_l);
        std::vector<signalPoint>().swap(pBG->aCell[i].sigPoints);
        for (int j = 0; j < this->params.num_bins_l; j++) {
            pBG->aCell[i].lCell[j].binPoints.clear();
            pBG->aCell[i].lCell[j].obsPoints.clear();
            pBG->aCell[i].lCell[j].drvPoints.clear();
            pBG->aCell[i].lCell[j].groundPoints.clear();
            pBG->aCell[i].lCell[j].prototypePoint = PointXYZGD();
            pBG->aCell[i].lCell[j].prototypePoint.x = NAN;
            pBG->aCell[i].lCell[j].prototypePoint.y = NAN;
            pBG->aCell[i].lCell[j].prototypePoint.z = NAN;
            pBG->aCell[i].rangeHeightSignal[j].x = NAN;
            pBG->aCell[i].rangeHeightSignal[j].y = NAN;
            pBG->aCell[i].lCell[j].cAssigned = -1;

            // force memory deletion of std::vectors
            std::vector<PointXYZGD>().swap(pBG->aCell[i].lCell[j].binPoints);
            std::vector<PointXYZGD>().swap(pBG->aCell[i].lCell[j].obsPoints);
            std::vector<PointXYZGD>().swap(pBG->aCell[i].lCell[j].drvPoints);
            std::vector<PointXYZGD>().swap(pBG->aCell[i].lCell[j].groundPoints);
        }
    }
}

void groundSegmentation::setupGroundSegmentation(
  pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud,
  pcl::PointCloud<PointXYZGD>::Ptr groundCloud,
  pcl::PointCloud<PointXYZGD>::Ptr obsCloud,
  pcl::PointCloud<PointXYZGD>::Ptr drvCloud) {
    groundCloud->clear();
    obsCloud->clear();

    refCloud = inputCloud;  // set the cloud datastructure;
    oCloud = obsCloud;      // set obs cloud
    gCloud = groundCloud;   // set ground cloud
    dCloud = drvCloud;

    oCloud->clear();
    gCloud->clear();
    dCloud->clear();

    genPolarBinGrid(refCloud);
}

void groundSegmentation::genPolarBinGrid(
  pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud) {
    initializePolarBinGrid();

    size_t nPts = inputCloud->size();
    double bsize_rad = (double) ((360.0) / this->params.num_bins_a);
    double bsize_lin = (double) this->params.rmax / this->params.num_bins_l;
    for (size_t i = 0; i < nPts; i++) {
        PointXYZGD curPt;
        float px = curPt.x = inputCloud->points[i].x;
        float py = curPt.y = inputCloud->points[i].y;
        float pz = curPt.z = inputCloud->points[i].z;

        if (sqrt(px * px + py * py + pz * pz) < this->params.rmax) {
            double ph = (atan2(py, px)) * (180 / M_PI);  // in degrees
            if (ph < 0)
                ph = 360.0 + ph;

            // bin into sector

            unsigned int bind_rad =
              static_cast<unsigned int>(ph / bsize_rad);  // got the radial bin

            if (bind_rad == this->params.num_bins_a) {
                bind_rad = 0;
            }

            // get the linear bin
            float xyDist = sqrt(px * px + py * py);

            unsigned int bind_lin = static_cast<unsigned int>(
              xyDist / bsize_lin);  // got the radial bin

            pBG->aCell[bind_rad].lCell[bind_lin].binPoints.push_back(curPt);
            // add the point to the bin
            // check the protoype point

            if (isnanf(pBG->aCell[bind_rad].lCell[bind_lin].prototypePoint.z) ||
                pz < pBG->aCell[bind_rad]
                       .lCell[bind_lin]
                       .prototypePoint.z)  // smallest by z
            {
                pBG->aCell[bind_rad].lCell[bind_lin].prototypePoint = curPt;
                pBG->aCell[bind_rad].rangeHeightSignal[bind_lin].x = xyDist;
                pBG->aCell[bind_rad].rangeHeightSignal[bind_lin].y = pz;
            }
        }
    }
}

MatX groundSegmentation::genGPModel(std::vector<signalPoint> &ps1,
                                    std::vector<signalPoint> &ps2,
                                    float sig_f,
                                    float p_l) {
    size_t nP1 = ps1.size();
    size_t nP2 = ps2.size();

    MatX CMAT;
    CMAT.resize(nP1, nP2);
    float coeff = (-1 / (2 * p_l * p_l));

    for (size_t i = 0; i < nP1; i++) {
        for (size_t j = 0; j < nP2; j++) {
            double diff = (ps1[i].range - ps2[j].range);
            CMAT(i, j) = sig_f * exp(coeff * (diff * diff));
        }
    }
    return CMAT;
}

void groundSegmentation::segmentGround() {
    for (int i = 0; i < this->params.num_bins_a; i++) {
        sectorINSAC(i);
    }
}

void groundSegmentation::sectorINSAC(int sectorIndex) {
    if (sectorIndex >= this->params.num_bins_a) {
        return;
    }
    int numFilled = 0;

    // pull out the valid points from the sector
    std::vector<signalPoint> &sigPtr = pBG->aCell[sectorIndex].sigPoints;
    sigPtr.clear();
    for (int i = 0; i < this->params.num_bins_l; i++) {
        if (!std::isnan(pBG->aCell[sectorIndex].rangeHeightSignal[i].x) &&
            pBG->aCell[sectorIndex].lCell[i].binPoints.size() > 5) {
            // bin has a valid point, and enough points to make a good
            // guess for a protopoint
            signalPoint newPoint;
            newPoint.range = pBG->aCell[sectorIndex].rangeHeightSignal[i].x;
            newPoint.height = pBG->aCell[sectorIndex].rangeHeightSignal[i].y;
            newPoint.idx = i;
            sigPtr.push_back(newPoint);
            numFilled++;
        }
    }
    // get the seed points.  Select the 3 lowest points.  Sort based on height
    // values
    sort(sigPtr.begin(), sigPtr.end(), compareSignalPoints);

    // now that the z points are sorted by height, take the
    // this->params.num_seed_points worth
    // as the seed
    size_t npt =
      sigPtr.size() < static_cast<size_t>(this->params.num_seed_points)
        ? sigPtr.size()
        : static_cast<size_t>(this->params.num_seed_points);
    std::vector<signalPoint> currentModel;
    int ptCtr = 0;
    int currIdx = 0;
    bool keepGoing = true;
    bool sufficientModel = true;

    while (true) {
        if (static_cast<size_t>(currIdx) >= sigPtr.size())  // overflow
        {
            break;
        }

        if (sigPtr[currIdx].range < this->params.max_seed_range &&
            fabs(sigPtr[currIdx].height) < this->params.max_seed_height) {
            // close enough to
            // robot and height
            // makese sense in
            // robot locality

            sigPtr[currIdx].isGround = true;
            currentModel.push_back(sigPtr[currIdx]);
            sigPtr.erase(sigPtr.begin() + currIdx);
            ptCtr++;

        } else {
            currIdx++;
        }

        if (static_cast<size_t>(ptCtr) >= npt)  // done
        {
            break;
        }
    }

    // check size
    if (currentModel.size() < 2)  // not enough for model, all obs pts
    {
        keepGoing = false;
        sufficientModel = false;
    }

    // got the seedpoints, start theINSAC process
    // cov matrices
    MatX C_XsX;
    MatX C_XX;
    MatX C_XsXs;
    MatX C_XXs;

    if (sigPtr.size() == 0)
        // no points to insac, put the seed points in as ground
        keepGoing = false;

    MatX temp;
    MatX f_s;
    MatX Vf_s;
    while (keepGoing) {
        // generate the covariance matrices

        C_XsX =
          genGPModel(sigPtr, currentModel, this->params.p_sf, this->params.p_l);
        C_XX = genGPModel(
          currentModel, currentModel, this->params.p_sf, this->params.p_l);
        C_XsXs =
          genGPModel(sigPtr, sigPtr, this->params.p_sf, this->params.p_l);
        C_XXs = C_XsX.transpose();

        // temporary calc
        MatX tCalc1 =
          C_XX + (this->params.p_sn * MatX::Identity(C_XX.rows(), C_XX.cols()));
        MatX tCalc2 = C_XsX * tCalc1.inverse();

        // test the points against the current model

        MatX modelZ(currentModel.size(), 1);
        for (unsigned int i = 0; i < currentModel.size(); i++) {
            modelZ(i, 0) = currentModel[i].height;
        }

        f_s = tCalc2 * modelZ;
        Vf_s = C_XsXs - tCalc2 * C_XXs;

        if (Vf_s.rows() == 0) {
            keepGoing = false;
            LOG_INFO("WARNING BREAKING LOOP: VF_s does not exist");
            continue;
        }

        bool searchCandidatePoints = true;
        unsigned int k = 0;
        // test for inliers using INSAC algorithm
        int startSize = currentModel.size();  // beginning size of the model set
        while (searchCandidatePoints) {
            double vf = Vf_s(k, k);
            double met =
              (sigPtr[k].height - f_s(k)) / (sqrt(this->params.p_sn + vf * vf));

            if (vf < this->params.p_tmodel &&
                abs(met) < this->params.p_tdata) {  // we have an inlier!
                // add to model set
                currentModel.push_back(sigPtr[k]);
                // remove from sample set
                sigPtr.erase(sigPtr.begin() + k);

                // delete row from f_s
                temp = f_s;
                f_s.resize(f_s.rows() - 1, f_s.cols());
                f_s.topRows(k) = temp.topRows(k);
                f_s.bottomRows(temp.rows() - k - 1) =
                  temp.bottomRows(temp.rows() - k - 1);

                // delete row from Vf_s
                temp = Vf_s;
                Vf_s.resize(Vf_s.rows() - 1, Vf_s.cols());
                Vf_s.topRows(k) = temp.topRows(k);
                Vf_s.bottomRows(temp.rows() - k - 1) =
                  temp.bottomRows(temp.rows() - k - 1);

                // delete col from Vf_s
                temp = Vf_s;
                Vf_s.resize(Vf_s.rows(), Vf_s.cols() - 1);
                Vf_s.leftCols(k) = temp.leftCols(k);
                Vf_s.rightCols(temp.cols() - k - 1) =
                  temp.rightCols(temp.cols() - k - 1);

            } else {
                k++;
            }

            if (sigPtr.size() == k) {
                searchCandidatePoints = false;
            }
        }

        int endSize = currentModel.size();  // end size of the model set
        if (startSize == endSize || sigPtr.size() == 0) {
            keepGoing = false;
        }
    }  // end INSAC

    // fill in the ground and obs pointclouds

    double numObs = 0;
    Vec3 obsSum(0, 0, 0);

    for (int i = 0; i < (int) currentModel.size(); i++) {
        int currIdx = currentModel[i].idx;
        linCell *curCell = &pBG->aCell[sectorIndex].lCell[currIdx];

        // go through all the points in this cell and assign to ground/not
        // ground
        for (unsigned int j = 0; j < curCell->binPoints.size(); j++) {
            float h = abs(currentModel[i].height - curCell->binPoints[j].z);
            if (h < this->params.p_tg)  // z heights are close
            {
                gCloud->push_back(
                  curCell->binPoints[j]);  // add the point to ground
                curCell->groundPoints.push_back(curCell->binPoints[j]);
            } else {
                // check drivability
                if (h > this->params.robot_height) {
                    // drivable
                    curCell->binPoints[j].drivable = 1;
                } else {
                    curCell->binPoints[j].drivable = 0;
                    dCloud->push_back(curCell->binPoints[j]);  // add to obs
                    curCell->drvPoints.push_back(curCell->binPoints[j]);
                }
                oCloud->push_back(curCell->binPoints[j]);  // add to obs
                curCell->obsPoints.push_back(curCell->binPoints[j]);
                obsSum += Vec3(curCell->binPoints[j].x,
                               curCell->binPoints[j].y,
                               curCell->binPoints[j].z);
                numObs++;
            }
        }
        // mean of obs points
        curCell->obsMean = obsSum / numObs;
    }

    // FIXME: WHY IS F_S < SIGPTR SOMETIMES?
    int i;
    if (sufficientModel) {
        // add all the obs points from the non ground classified pts
        for (i = 0; i < (int) sigPtr.size(); i++) {
            linCell *curCell = &pBG->aCell[sectorIndex].lCell[sigPtr[i].idx];

            for (int j = 0; j < (int) curCell->binPoints.size(); j++) {
                float h = abs(curCell->binPoints[j].z - f_s(i));
                // check drivability
                if (h > this->params.robot_height) {
                    // drivable
                    curCell->binPoints[j].drivable = 1;
                } else {
                    curCell->binPoints[j].drivable = 0;
                    dCloud->push_back(curCell->binPoints[j]);  // add to obs
                    curCell->drvPoints.push_back(curCell->binPoints[j]);
                }
                oCloud->push_back(curCell->binPoints[j]);  // add to obs
                curCell->obsPoints.push_back(curCell->binPoints[j]);
                obsSum += Vec3(curCell->binPoints[j].x,
                               curCell->binPoints[j].y,
                               curCell->binPoints[j].z);
                numObs++;
            }
            // mean of obs points
            curCell->obsMean = obsSum / numObs;
        }
    } else {
        LOG_INFO("WARNING:Insufficnent Model for angular slice");
    }
}

}  // namespace wave


#ifdef WIN32
#define NOMINMAX
#endif
#include <float.h>
#include <omp.h>
#include <time.h>
#include <algorithm>
#include <chrono>
#include <fstream>
#include <iostream>
#include <numeric>
#include <queue>
#include <set>
#include <stack>
#include <unordered_map>

#include <misc3d/features/boundary_detection.h>
#include <misc3d/logging.h>
#include <misc3d/pose_estimation/data_structure.h>
#include <misc3d/pose_estimation/ppf_estimation.h>
#include <misc3d/utils.h>
#include <open3d/geometry/BoundingVolume.h>
#include <open3d/geometry/KDTreeSearchParam.h>
#include <open3d/pipelines/registration/Registration.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

namespace misc3d {

namespace pose_estimation {

inline bool IsNormalizedVector(PointXYZ &v) {
    const double norm = v.norm();
    if (norm > 1.0e-6) {
        for (int i = 0; i < 3; i++) {
            v(i) *= 1 / norm;
        }
        return true;
    }
    return false;
}

class PPFEstimator::Impl {
public:
    Impl(const PPFEstimatorConfig &config) {
        config_ = config;
        dist_threshold_ = config.rel_dist_thresh_;
        // (calc_normal_relative_ = config.rel_sample_dist_ / 2) is usually a
        // good choise
        calc_normal_relative_ = config.training_param_.calc_normal_relative;
        enable_edge_support_ = (config.voting_param_.method ==
                                PPFEstimatorConfig::VotingMode::EdgePoints)
                                   ? true
                                   : false;
        invert_normal_ = config.training_param_.invert_model_normal;
    }

    ~Impl() {}

    bool SetConfig(const PPFEstimatorConfig &config);

    bool Train(const PointCloudPtr &pc);

    bool Estimate(const PointCloudPtr &pc, std::vector<Pose6D> &results);

private:
    void PreprocessTrain(const PointCloudPtr &pc,
                         open3d::geometry::PointCloud &pc_sample);

    void PreprocessEstimate(const PointCloudPtr &pc,
                            open3d::geometry::PointCloud &pc_sample);

    void CalcPPF(const PointXYZ &p0, const Normal &n0, const PointXYZ &p1,
                 const Normal &n1, std::array<double, 4> &ppf);

    void QuantizePPF(const std::array<double, 4> &ppf,
                     std::array<size_t, 4> &quant_ppf);

    size_t HashPPF(const std::array<size_t, 4> &ppf);

    size_t HashPPF(const std::array<double, 4> &ppf);

    Transformation CalcTNormal2RegionX(const PointXYZ &p, const Normal &n);

    double CalcAlpha(const PointXYZ &pt,
                     const Transformation t_normal_to_region);

    // use_flag: 1: use translation, 2: dsadse rotation, 3: both
    bool MatchPose(const Pose6D &src, const Pose6D &dst, const int use_flag);

    void SpreadPPF(const std::array<size_t, 4> &ppf,
                   std::array<size_t, 81> &idx, int &n);

    void ClusterPoses(std::vector<Pose6D> &pose_list,
                      std::vector<std::vector<Pose6D>> &clustered_pose);

    void RefineSparsePose(
        const open3d::geometry::PointCloud &model_pcn,
        const open3d::geometry::PointCloud &scene_pcn,
        const std::vector<std::vector<Pose6D>> &clustered_pose,
        std::vector<Pose6D> &pose_list);

    void CalcPoseNeighbor(const std::vector<Pose6D> &pose_list,
                          const std::vector<int> &indices, const int use_flag,
                          std::vector<std::vector<int>> &neibor_idx);

    void GatherPose(const std::vector<Pose6D> &pose_list,
                    const std::vector<int> &indices,
                    const std::vector<std::vector<int>> &neibor_idx,
                    std::vector<PoseCluster> &pose_clusters);

    void PoseAverage(const std::vector<Pose6D> &pose_list,
                     const std::vector<int> &pose_idx, Pose6D &result_pose);

    void FlipNormal(Normal &normal) { normal *= -1; }

    void CalcModelNormalAndSampling(const PointCloudPtr &pc,
                                    const std::shared_ptr<KDTree> &kdtree,
                                    const double normal_r, const double step,
                                    const PointXYZ &view_pt,
                                    open3d::geometry::PointCloud &pc_sample);

    void GenerateLUT();

    void GenerateModelPCNeighbor(const open3d::geometry::PointCloud &pts,
                                 std::vector<std::vector<int>> &neighbor_table,
                                 const double radius);

    void CalcLocalMaximum(const size_t *accumulator, const size_t rows,
                          const size_t cols, const size_t vote_threshold,
                          const double relative_to_max,
                          std::vector<std::vector<size_t>> &max_in_pair,
                          std::vector<std::vector<int>> &neighbor_table);

    void ExtractEdges(const open3d::geometry::PointCloud &pc, double radius,
                      std::vector<size_t> &edge_ind);

    void CalcHashTable(const open3d::geometry::PointCloud &reference_pts,
                       const open3d::geometry::PointCloud &refered_pts,
                       std::vector<std::vector<PointPair>> &hashtable_ptr,
                       std::vector<Transformation> &tmg_ptr,
                       bool b_same_pointset);

    void DownSamplePCNormal(const open3d::geometry::PointCloud &pc,
                            const std::shared_ptr<KDTree> &kdtree,
                            const double radius, const size_t min_idx,
                            open3d::geometry::PointCloud &pc_normal);

    void VotingAndGetPose(
        const open3d::geometry::PointCloud &reference_pts,
        const open3d::geometry::PointCloud &refered_pts,
        const std::vector<std::vector<PointPair>> &hashtable_ptr,
        const std::vector<Transformation> &tmg_ptr,
        std::vector<Pose6D> &pose_list, size_t reference_num_in_model,
        size_t refered_model_num,
        std::vector<std::vector<int>> &neighbor_table);

public:
    std::vector<Pose6D> pose_list_;
    std::vector<std::vector<Pose6D>> pose_clustered_by_t_;
    double diameter_;

    bool enable_edge_support_;
    bool invert_normal_;
    open3d::geometry::PointCloud dense_model_sample_;
    open3d::geometry::PointCloud dense_scene_sample_;
    open3d::geometry::PointCloud model_sample_;
    open3d::geometry::PointCloud scene_sample_;
    std::vector<size_t> scene_edge_ind_;
    std::vector<size_t> model_edge_ind_;

private:
    double dist_step_;
    double dist_threshold_;
    open3d::geometry::PointCloud model_sample_centered_;
    size_t pc_num_;
    std::vector<std::vector<PointPair>> hash_table_;
    std::vector<std::vector<PointPair>> hashtable_boundary_;
    double r_min_, r_max_;

    size_t reference_num_;
    size_t refered_num_;
    size_t hash_table_size_;
    size_t angle_num_, dist_num_;

    std::vector<Transformation> tmg_ptr_;
    std::vector<Transformation> tmg_ptr_boundary_;

    bool trained_;
    double calc_normal_relative_;
    PointXYZ centroid_;

    size_t angle_num_2_, angle_num_3_;
    std::vector<int> alpha_lut_;
    std::vector<std::array<int, 3>> alpha_shift_lut_;
    std::vector<std::array<int, 3>> dist_shift_lut_;
    std::vector<std::array<int, 3>> alpha_2_shift_lut_;

    std::vector<std::vector<int>> pc_neighbor_;

    double dist_step_dense_;

    PPFEstimatorConfig config_;
};

void PPFEstimator::Impl::PreprocessTrain(
    const PointCloudPtr &pc, open3d::geometry::PointCloud &pc_sample) {
    // compute bounding box of model
    const open3d::geometry::OrientedBoundingBox bbox =
        pc->GetOrientedBoundingBox();
    const Eigen::Vector3d d = bbox.extent_;
    std::vector<double> d_ordered{d(0), d(1), d(2)};

    const double diameter = sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]);
    diameter_ = diameter;

    // set view point in above of bbox center
    PointXYZ view_pt = bbox.GetCenter();
    view_pt(2) += VIEW_POINT_Z_EXTEND * diameter;

    // set parameters
    r_max_ = diameter;
    std::sort(d_ordered.begin(), d_ordered.end());
    r_min_ = sqrt(d_ordered[0] * d_ordered[0] + d_ordered[1] * d_ordered[1]);
    dist_step_ = diameter * config_.training_param_.rel_sample_dist;
    dist_step_dense_ = diameter * config_.training_param_.rel_dense_sample_dist;
    dist_threshold_ *= diameter;
    const double normal_r = diameter * calc_normal_relative_;

    misc3d::LogInfo(
        "Model training params: normal search radius: {}, sample "
        "distance: {}, dense sample distance: {}",
        normal_r, dist_step_, dist_step_dense_);

    auto kdtree = std::make_shared<KDTree>(*pc);
    CalcModelNormalAndSampling(pc, kdtree, normal_r, dist_step_, view_pt,
                               pc_sample);

    misc3d::LogInfo("Model sample point number is {} | {} after preprocessing",
                    pc_sample.points_.size(), pc->points_.size());
}

void PPFEstimator::Impl::PreprocessEstimate(
    const PointCloudPtr &pc, open3d::geometry::PointCloud &pc_sample) {
    Timer timer;
    timer.Start();

    if (!pc->HasPoints()) {
        misc3d::LogError("There is no scene point clouds.");
        return;
    }

    const bool has_normals = pc->HasNormals();
    const double normal_radius = calc_normal_relative_ * diameter_;
    pc->RemoveNonFinitePoints();
    if (!has_normals) {
        pc->EstimateNormals(open3d::geometry::KDTreeSearchParamHybrid(
            normal_radius, NORMAL_CALC_NN));
    }
    NormalConsistent(*pc);

    const size_t num = pc->points_.size();
    pc_sample = *pc->VoxelDownSample(dist_step_);

    misc3d::LogInfo("Scene point number is {} | {} after preprocessing.",
                    pc_sample.points_.size(), num);

    if (enable_edge_support_) {
        dense_scene_sample_ = *pc->VoxelDownSample(dist_step_dense_);

        scene_edge_ind_.clear();
        const double radius = diameter_ * calc_normal_relative_;
        ExtractEdges(dense_scene_sample_, radius, scene_edge_ind_);
    }

    misc3d::LogInfo("Preprocess time cost for estimate stage: {}",
                    timer.Stop());
}

bool PPFEstimator::Impl::Estimate(const PointCloudPtr &pc,
                                  std::vector<Pose6D> &results) {
    PreprocessEstimate(pc, scene_sample_);

    results.clear();
    Timer timer;
    timer.Start();

    if (!trained_) {
        misc3d::LogError("Need training before estimating!");
        return false;
    }

    if (!scene_sample_.HasPoints()) {
        return false;
    }

    if (config_.voting_param_.method ==
        PPFEstimatorConfig::VotingMode::EdgePoints) {
        if (scene_edge_ind_.empty()) {
            return false;
        }
    }

    // currently only supporting key points random selection
    const size_t num = scene_sample_.points_.size();
    RandomSampler<size_t> sampler(num);
    const size_t sample = config_.ref_param_.ratio * num;
    std::vector<size_t> keypoints_indices =
        sampler.SampleWithoutDuplicate(sample);

    const auto key_points = scene_sample_.SelectByIndex(keypoints_indices);
    std::vector<Pose6D> pose_list;
    if (config_.voting_param_.method ==
        PPFEstimatorConfig::VotingMode::SampledPoints) {
        VotingAndGetPose(*key_points, scene_sample_, hash_table_, tmg_ptr_,
                         pose_list, pc_num_, pc_num_, pc_neighbor_);
    }

    if (config_.voting_param_.method ==
        PPFEstimatorConfig::VotingMode::EdgePoints) {
        const auto scene_boundary_points =
            dense_scene_sample_.SelectByIndex(scene_edge_ind_);

        // we use sampled key points and edge points for point pair computation
        VotingAndGetPose(*key_points, *scene_boundary_points,
                         hashtable_boundary_, tmg_ptr_boundary_, pose_list,
                         pc_num_, model_edge_ind_.size(), pc_neighbor_);
    }

    if (pose_list.empty()) {
        return false;
    } else {
        misc3d::LogInfo("Find {} raw poses after voting.", pose_list.size());
    }

    std::vector<std::vector<Pose6D>> pose_cluster;
    ClusterPoses(pose_list, pose_cluster);

    RefineSparsePose(model_sample_centered_, scene_sample_, pose_cluster,
                     results);

    // perform center shifting
    auto TransformOrigin = [&](Pose6D &pose) {
        Transformation T = pose.pose_;
        for (int i = 0; i < 3; i++) {
            T(i, 3) -= centroid_(0) * T(i, 0) + centroid_(1) * T(i, 1) +
                       centroid_(2) * T(i, 2);
        }
        pose.UpdateByPose(T);
    };
    std::for_each(results.begin(), results.end(), TransformOrigin);

    // sort pose based on number of votes
    std::sort(results.begin(), results.end(), [](Pose6D &p0, Pose6D &p1) {
        return p0.num_votes_ > p1.num_votes_;
    });

    double expected_votes_num =
        config_.ref_param_.ratio * reference_num_ * refered_num_;

    if (config_.voting_param_.method ==
        PPFEstimatorConfig::VotingMode::SampledPoints) {
        expected_votes_num *= VOTES_NUM_REDUCTION_FACTOR;
    }

    for (size_t i = 0; i < results.size(); i++) {
        results[i].object_id_ = config_.object_id_;
        const double score =
            std::min(1.0, (double)results[i].num_votes_ / expected_votes_num);
        results[i].score_ = score;
        misc3d::LogInfo("Pose {} with score {}", i, score);
    }

    // remove pose which votes is less than threshold
    size_t valid_num = 0;
    for (; valid_num < results.size();) {
        if (results[valid_num].score_ < config_.score_thresh_) {
            break;
        }
        valid_num++;
    }
    results.erase(results.begin() + valid_num, results.end());
    misc3d::LogInfo("Find {} pose satisfy score threshold", valid_num);

    if (results.empty()) {
        return false;
    }

    // remove pose by number of output
    if (valid_num > config_.num_result_) {
        results.erase(results.begin() + config_.num_result_, results.end());
    }
    pose_list_.assign(results.begin(), results.end());

    misc3d::LogInfo("Estimating time cost: {}", timer.Stop());
    return true;
}

void PPFEstimator::Impl::VotingAndGetPose(
    const open3d::geometry::PointCloud &reference_pts,
    const open3d::geometry::PointCloud &refered_pts,
    const std::vector<std::vector<PointPair>> &hashtable_ptr,
    const std::vector<Transformation> &tmg_ptr, std::vector<Pose6D> &pose_list,
    size_t reference_model_num, size_t refered_model_num,
    std::vector<std::vector<int>> &neighbor_table) {
    pose_list.clear();
    const size_t reference_num = reference_pts.points_.size();
    const size_t refered_num = refered_pts.points_.size();
    misc3d::LogInfo("Reference num: {}, Refered num: {}", reference_num,
                    refered_num);

    const size_t votes_threshold = refered_model_num * VOTING_THRESHOLD_FACTOR;
    const int alpha_model_num = angle_num_ * 2 - 1;
    const double dist_thresh = config_.voting_param_.min_dist_thresh * r_min_;
    const double cos_angle_thresh = cos(config_.voting_param_.min_angle_thresh);
    const double vote_num_relative_to_max = VOTE_NUM_RATIO;

    const size_t acc_size = reference_model_num * alpha_model_num;

    KDTree kdtree(refered_pts);
#pragma omp parallel for
    for (int si = 0; si < reference_num; si++) {
        std::array<double, 4> ppf;
        std::array<size_t, 4> quantized_ppf;
        std::array<size_t, 81> spread_idx;

        double alpha_scene_rad, alpha;
        size_t quantize_alpha, idx;
        int n_searched, alpha_index, n_ppfs;
        size_t alpha_scene_bit, t;

        Transformation model_pose, tsg_inv, tmg;

        const PointXYZ &p0_p = reference_pts.points_[si];
        const Normal &p0_n = reference_pts.normals_[si];

        size_t max_votes, corr_alpha_index, corr_model_index;

        std::vector<size_t> accumulator(acc_size, 0);
        std::vector<size_t> flags_b(hash_table_size_, 0);

        Transformation tsg = CalcTNormal2RegionX(p0_p, p0_n);

        std::vector<int> ret_indices;
        std::vector<double> out_dists_sqr;

        n_searched =
            kdtree.SearchRadius(p0_p, r_min_, ret_indices, out_dists_sqr);

        if (n_searched > votes_threshold) {
            for (int j = 0; j < n_searched; j++) {
                const PointXYZ &p1_p = refered_pts.points_[ret_indices[j]];
                const Normal &p1_n = refered_pts.normals_[ret_indices[j]];
                // filter out the angle between normal which less than
                // angle threshold
                if (sqrt(out_dists_sqr[j]) < dist_thresh &&
                    p1_n.dot(p0_n) > cos_angle_thresh) {
                    continue;
                }

                alpha_scene_rad = CalcAlpha(p1_p, tsg);
                quantize_alpha = round((alpha_scene_rad + M_PI) /
                                       config_.voting_param_.angle_step);
                alpha_scene_bit = 1 << quantize_alpha;
                CalcPPF(p0_p, p0_n, p1_p, p1_n, ppf);
                QuantizePPF(ppf, quantized_ppf);
                SpreadPPF(quantized_ppf, spread_idx, n_ppfs);

                for (int ni = 0; ni < n_ppfs; ni++) {
                    idx = spread_idx[ni];
                    t = flags_b[idx] | alpha_scene_bit;
                    if (t == flags_b[idx]) {
                        continue;
                    }

                    flags_b[idx] = t;
                    const int n_vals = hashtable_ptr[idx].size();
                    for (int hi = 0; hi < n_vals; hi++) {
                        const PointPair &pp = hashtable_ptr[idx][hi];
                        alpha_index =
                            alpha_lut_[pp.quant_alpha_ * alpha_model_num +
                                       quantize_alpha];
                        accumulator[pp.i_ * alpha_model_num + alpha_index]++;
                    }
                }
            }
            // 0: model id, 1: quantized angle id, 2: vote number
            std::vector<std::vector<size_t>> local_maximum;

            CalcLocalMaximum(accumulator.data(), reference_model_num,
                             alpha_model_num, votes_threshold,
                             vote_num_relative_to_max, local_maximum,
                             neighbor_table);

            if (local_maximum.size() > 0) {
                // CalcMatrixInverse(tsg, tsg_inv);
                tsg_inv = tsg.inverse();
            } else {
                continue;
            }

            for (int li = 0; li < local_maximum.size(); li++) {
                Pose6D pose;
                corr_model_index = local_maximum[li][0];
                corr_alpha_index = local_maximum[li][1];
                max_votes = local_maximum[li][2];

                alpha = corr_alpha_index * config_.voting_param_.angle_step;
                double ca = cos(alpha), sa = sin(alpha);
                Transformation tr;
                tr << 1, 0, 0, 0, 0, ca, -sa, 0, 0, sa, ca, 0, 0, 0, 0, 1;

                tmg = tmg_ptr[corr_model_index];

                model_pose = tsg_inv * tr * tmg;
                pose.UpdateByPose(model_pose);
                pose.num_votes_ = max_votes;
                pose.corr_mi_ = corr_model_index;
#pragma omp critical
                { pose_list.emplace_back(pose); }
            }
        }
    }
}

bool PPFEstimator::Impl::Train(const PointCloudPtr &pc) {
    Timer timer;
    timer.Start();
    if (!pc->HasPoints()) {
        misc3d::LogError("There is no input points");
        return false;
    }

    // preprocess input point and compute normal
    PreprocessTrain(pc, model_sample_);

    // set ppf look-up table parameters
    angle_num_ = round(M_PI / config_.voting_param_.angle_step) + 1;
    angle_num_2_ = angle_num_ * angle_num_;
    angle_num_3_ = angle_num_2_ * angle_num_;
    dist_num_ = round(1 / config_.training_param_.rel_sample_dist) + 1;
    hash_table_size_ = pow(angle_num_, 3) * dist_num_;
    pc_num_ = model_sample_.points_.size();

    if (pc_num_ == 0) {
        misc3d::LogError("There is no input points after preprocessing");
        return false;
    }

    // shift model origin to centroid for easy pose clustering
    centroid_.setZero();
    for (int i = 0; i < pc_num_; i++) {
        centroid_ += model_sample_.points_[i];
    }
    centroid_ /= static_cast<double>(pc_num_);
    const size_t data_length = sizeof(double) * 3;
    model_sample_centered_ = model_sample_;
    for (int i = 0; i < pc_num_; i++) {
        model_sample_centered_.points_[i] -= centroid_;
    }

    hash_table_.resize(hash_table_size_);
    CalcHashTable(model_sample_centered_, model_sample_centered_, hash_table_,
                  tmg_ptr_, true);

    GenerateModelPCNeighbor(model_sample_centered_, pc_neighbor_,
                            r_min_ * NEIGHBOR_RADIUS_FACTOR);

    reference_num_ = model_sample_centered_.points_.size();
    refered_num_ = model_sample_centered_.points_.size();

    if (enable_edge_support_) {
        open3d::geometry::PointCloud dense_model_sample_centered =
            dense_model_sample_;

        if (model_edge_ind_.empty()) {
            misc3d::LogError("Fail to extract edge points!");
            return false;
        }

        for (size_t i = 0; i < dense_model_sample_centered.points_.size();
             i++) {
            dense_model_sample_centered.points_[i] -= centroid_;
        }

        hashtable_boundary_.resize(hash_table_size_);

        const auto model_edge_pts =
            dense_model_sample_.SelectByIndex(model_edge_ind_);

        CalcHashTable(model_sample_centered_, *model_edge_pts,
                      hashtable_boundary_, tmg_ptr_boundary_, false);
    }

    // generate ppf look-up table
    GenerateLUT();
    trained_ = true;

    misc3d::LogInfo("Training time cost: {}", timer.Stop());
    return true;
}

void PPFEstimator::Impl::CalcHashTable(
    const open3d::geometry::PointCloud &reference_pts,
    const open3d::geometry::PointCloud &refered_pts,
    std::vector<std::vector<PointPair>> &hashtable_ptr,
    std::vector<Transformation> &tmg_ptr, bool b_same_pointset) {
    const size_t reference_num = reference_pts.points_.size();
    const size_t refered_num = refered_pts.points_.size();
    const int double_size = sizeof(double);
    tmg_ptr.resize(reference_num);

#pragma omp parallel for
    for (int i = 0; i < reference_num; i++) {
        size_t idx;
        const PointXYZ &p0_p = reference_pts.points_[i];
        const Normal &p0_n = reference_pts.normals_[i];

        const Transformation tmg = CalcTNormal2RegionX(p0_p, p0_n);
        tmg_ptr[i] = tmg;

        std::array<double, 4> ppf;
        for (size_t j = 0; j < refered_num; j++) {
            if ((!b_same_pointset) || (b_same_pointset && j != i)) {
                PointPair pp;
                const PointXYZ &p1_p = refered_pts.points_[j];
                const Normal &p1_n = refered_pts.normals_[j];
                CalcPPF(p0_p, p0_n, p1_p, p1_n, ppf);
                pp.i_ = i;
                pp.j_ = j;
                pp.alpha_ = CalcAlpha(p1_p, tmg);
                pp.quant_alpha_ = round((pp.alpha_ + M_PI) /
                                        config_.voting_param_.angle_step);
                idx = HashPPF(ppf);
#pragma omp critical
                { hashtable_ptr[idx].emplace_back(pp); }
            }
        }
    }
}

void PPFEstimator::Impl::CalcPPF(const PointXYZ &p0, const Normal &n0,
                                 const PointXYZ &p1, const Normal &n1,
                                 std::array<double, 4> &ppf) {
    Eigen::Vector3d d(p1(0) - p0(0), p1(1) - p0(1), p1(2) - p0(2));
    const double norm = d.norm();
    d.normalize();

    ppf[0] = acos(n0.dot(d));
    ppf[1] = acos(n1.dot(d));
    ppf[2] = acos(n0.dot(n1));
    ppf[3] = norm;
}

void PPFEstimator::Impl::QuantizePPF(const std::array<double, 4> &ppf,
                                     std::array<size_t, 4> &quant_ppf) {
    for (int i = 0; i < 3; i++) {
        quant_ppf[i] = round(ppf[i] / config_.voting_param_.angle_step);
    }
    quant_ppf[3] = round(ppf[3] / dist_step_);
}

size_t PPFEstimator::Impl::HashPPF(const std::array<size_t, 4> &ppf) {
    return ppf[0] + ppf[1] * angle_num_ + ppf[2] * angle_num_2_ +
           ppf[3] * angle_num_3_;
}

size_t PPFEstimator::Impl::HashPPF(const std::array<double, 4> &ppf) {
    std::array<size_t, 4> quantized_ppf;
    QuantizePPF(ppf, quantized_ppf);
    return HashPPF(quantized_ppf);
}

Transformation PPFEstimator::Impl::CalcTNormal2RegionX(const PointXYZ &p,
                                                       const Normal &n) {
    PointXYZ u(0, n(2), -n(1));
    if (!IsNormalizedVector(u)) {
        u(1) = 1;
        u(2) = 0;
    }

    // quaternion to rotate matrix
    const double half_alpha_rad = acos(n(0)) / 2;
    const Eigen::Vector4d quat(cos(half_alpha_rad), 0,
                               sin(half_alpha_rad) * u(1),
                               sin(half_alpha_rad) * u(2));

    Rotation rotation;
    Pose6D::QuatToMat(quat, rotation);

    Transformation tg;
    tg.setIdentity();
    tg.block<3, 1>(0, 3) = -1 * rotation * p;
    tg.block<3, 3>(0, 0) = rotation;

    return tg;
}

double PPFEstimator::Impl::CalcAlpha(const PointXYZ &pt,
                                     const Transformation t_normal_to_region) {
    const auto tran_pt = t_normal_to_region.block<3, 3>(0, 0) * pt +
                         t_normal_to_region.block<3, 1>(0, 3);
    return atan2(-tran_pt(2), tran_pt(1));
}

void PPFEstimator::Impl::SpreadPPF(const std::array<size_t, 4> &ppf,
                                   std::array<size_t, 81> &idx, int &n) {
    int d, a0, a1, a2;
    n = 0;

    int k;

    if (config_.voting_param_.faster_mode) {
        k = 2;
    } else {
        k = 3;
    }

    for (int s0 = 0; s0 < 3; s0++) {
        d = dist_shift_lut_[ppf[3]][s0] * angle_num_3_;
        if (d < 0)
            continue;
        for (int s1 = 0; s1 < k; s1++) {
            a0 = alpha_shift_lut_[ppf[0]][s1];
            if (a0 < 0)
                continue;
            a0 += d;
            for (int s2 = 0; s2 < k; s2++) {
                a1 = alpha_shift_lut_[ppf[1]][s2] * angle_num_;
                if (a1 < 0)
                    continue;
                a1 += a0;
                for (int s3 = 0; s3 < k; s3++) {
                    a2 = alpha_shift_lut_[ppf[2]][s3] * angle_num_2_;
                    if (a2 < 0)
                        continue;
                    idx[n] = a2 + a1;
                    n++;
                }
            }
        }
    }
}

bool PPFEstimator::Impl::MatchPose(const Pose6D &src, const Pose6D &dst,
                                   const int use_flag) {
    const auto &src_pose = src.pose_;
    const auto &dst_pose = dst.pose_;
    bool matched = true;
    if (use_flag & 1) {
        const PointXYZ t =
            dst_pose.block<3, 1>(0, 3) - src_pose.block<3, 1>(0, 3);
        const double norm_2 = t.norm();
        matched = (norm_2 < dist_threshold_);
    }
    if (use_flag & 2 && matched) {
        double trace = 0;
        for (int r = 0; r < 3; r++) {
            for (int c = 0; c < 3; c++) {
                trace += src_pose(r, c) * dst_pose(r, c);
            }
        }

        double ca = (trace - 1) / 2;
        double angle_rad;
        if (ca > 1) {
            angle_rad = 0;
        } else if (ca < -1) {
            angle_rad = M_PI;
        } else {
            angle_rad = acos(ca);
        }
        matched = (angle_rad < config_.rel_angle_thresh_);
    }
    return matched;
}

void PPFEstimator::Impl::GatherPose(
    const std::vector<Pose6D> &pose_list, const std::vector<int> &indices,
    const std::vector<std::vector<int>> &neibor_idx,
    std::vector<PoseCluster> &pose_clusters) {
    pose_clusters.clear();
    const size_t n_pose = neibor_idx.size();
    if (n_pose == 0)
        return;
    // int min_num_pt = 10;  // double min_num_pt
    int min_num_pt = 6;  // double min_num_pt
    for (size_t i = 0, num; i < n_pose; i++) {
        num = neibor_idx[i].size();
        if (num > min_num_pt) {
            min_num_pt = num;
        }
    }
    min_num_pt /= 2.0;
    std::vector<bool> check_flags(pose_list.size(), false);
    size_t n_cluster = 0;
    size_t mi, ci;
    for (size_t pi = 0; pi < n_pose; pi++) {
        ci = pi;
        mi = indices[ci];
        if (check_flags[mi] || neibor_idx[ci].size() < min_num_pt)
            continue;
        pose_clusters.emplace_back(PoseCluster());
        std::stack<size_t> my_stack;
        my_stack.push(ci);
        while (!my_stack.empty()) {
            ci = my_stack.top();
            mi = indices[ci];
            my_stack.pop();
            if (check_flags[mi])
                continue;
            pose_clusters[n_cluster].pose_indices_.emplace_back(mi);
            pose_clusters[n_cluster].num_votes_ += pose_list[mi].num_votes_;
            check_flags[mi] = true;
            for (size_t ni : neibor_idx[ci]) {
                ci = ni;
                mi = indices[ci];
                if (check_flags[mi])
                    continue;
                if (neibor_idx[ci].size() >= min_num_pt) {
                    my_stack.push(ci);
                } else {
                    pose_clusters[n_cluster].pose_indices_.emplace_back(mi);
                    pose_clusters[n_cluster].num_votes_ +=
                        pose_list[mi].num_votes_;
                    check_flags[mi] = true;
                }
            }
        }
        n_cluster++;
    }
    sort(pose_clusters.begin(), pose_clusters.end(),
         [](PoseCluster &p0, PoseCluster &p1) {
             return p0.num_votes_ > p1.num_votes_;
         });
}

void PPFEstimator::Impl::CalcPoseNeighbor(
    const std::vector<Pose6D> &pose_list, const std::vector<int> &indices,
    const int use_flag, std::vector<std::vector<int>> &neibor_idx) {
    neibor_idx.clear();
    const size_t n_pose = indices.size();
    if (n_pose == 0)
        return;
    std::vector<std::vector<bool>> match_table(
        n_pose, std::vector<bool>(n_pose, false));
#pragma omp parallel for
    for (int i = 0; i < n_pose; i++) {
        bool matched;
        const Pose6D *center_pose, *cur_pose;
        center_pose = &pose_list[indices[i]];
        match_table[i][i] = true;
        for (size_t j = i + 1; j < n_pose; j++) {
            cur_pose = &pose_list[indices[j]];
            matched = MatchPose(*center_pose, *cur_pose, use_flag);
            match_table[i][j] = matched;
            match_table[j][i] = matched;
        }
    }
    neibor_idx.resize(n_pose);
#pragma omp parallel for
    for (int pi = 0; pi < n_pose; pi++) {
        for (size_t j = 0; j < n_pose; j++) {
            if (match_table[pi][j]) {
                neibor_idx[pi].emplace_back(j);
            }
        }
    }
}

void PPFEstimator::Impl::ClusterPoses(
    std::vector<Pose6D> &pose_list,
    std::vector<std::vector<Pose6D>> &clustered_pose) {
    clustered_pose.clear();
    std::sort(pose_list.begin(), pose_list.end(), [](Pose6D &p0, Pose6D &p1) {
        return p0.num_votes_ > p1.num_votes_;
    });
    const size_t n_pose = pose_list.size();

    const size_t num_votes_threshold = pose_list[0].num_votes_ * 0.5;
    size_t valid_pose = 0;
    for (; valid_pose < n_pose; valid_pose++) {
        if (pose_list[valid_pose].num_votes_ < num_votes_threshold) {
            break;
        }
    }
    // use kdtree to find neighbor
    open3d::geometry::PointCloud pose_ts;
    pose_ts.points_.resize(valid_pose);
    for (size_t i = 0; i < valid_pose; i++) {
        pose_ts.points_[i] = pose_list[i].t_;
    }

    KDTree kdtree(pose_ts);

    std::vector<int> ret_indices;
    std::vector<double> out_dists_sqr;
    int n_searched;
    std::vector<std::vector<int>> neighbor_indexs(valid_pose);
    std::vector<int> indices(valid_pose);
    for (int i = 0; i < valid_pose; i++) {
        indices[i] = i;
        const PointXYZ &temp = pose_ts.points_[i];
        n_searched = kdtree.SearchRadius(temp, dist_threshold_, ret_indices,
                                         out_dists_sqr);
        neighbor_indexs[i] = ret_indices;
    }
    std::vector<PoseCluster> pose_cluster;
    GatherPose(pose_list, indices, neighbor_indexs, pose_cluster);
    const size_t n_cluster = pose_cluster.size();
    clustered_pose.resize(n_cluster);
    pose_clustered_by_t_.resize(n_cluster);

#pragma omp parallel for
    for (int i = 0; i < n_cluster; i++) {
        std::vector<std::vector<int>> temp_neighbor_indices;

        for (size_t index : pose_cluster[i].pose_indices_) {
            pose_clustered_by_t_[i].emplace_back(pose_list[index]);
        }

        CalcPoseNeighbor(pose_list, pose_cluster[i].pose_indices_, 3,
                         temp_neighbor_indices);

        std::vector<PoseCluster> temp_pose_cluster;
        GatherPose(pose_list, pose_cluster[i].pose_indices_,
                   temp_neighbor_indices, temp_pose_cluster);

        clustered_pose[i].resize(temp_pose_cluster.size());
        for (int j = 0; j < temp_pose_cluster.size(); j++) {
            PoseAverage(pose_list, temp_pose_cluster[j].pose_indices_,
                        clustered_pose[i][j]);
        }
    }
}

void PPFEstimator::Impl::RefineSparsePose(
    const open3d::geometry::PointCloud &model_pcn,
    const open3d::geometry::PointCloud &scene_pcn,
    const std::vector<std::vector<Pose6D>> &clustered_pose,
    std::vector<Pose6D> &pose_list) {
    const double max_correspondence_distance =
        config_.refine_param_.rel_dist_sparse_thresh * dist_step_;
    const open3d::pipelines::registration::ICPConvergenceCriteria criteria(
        1e-6, 1e-6, SPARSE_REFINE_ICP_ITERATION);
#pragma omp parallel for
    for (int i = 0; i < clustered_pose.size(); i++) {
        if (clustered_pose[i].size() == 0) {
            continue;
        }

        const Pose6D max_votes_pose = *std::max_element(
            clustered_pose[i].begin(), clustered_pose[i].end(),
            [](const Pose6D &p1, const Pose6D &p2) {
                return p1.num_votes_ < p2.num_votes_;
            });

        const auto &init_pose = max_votes_pose.pose_;

        open3d::pipelines::registration::RegistrationResult res;
        if (config_.refine_param_.method ==
            PPFEstimatorConfig::RefineMethod::PointToPoint) {
            res = open3d::pipelines::registration::RegistrationICP(
                model_pcn, scene_pcn, max_correspondence_distance, init_pose,
                open3d::pipelines::registration::
                    TransformationEstimationPointToPoint(),
                criteria);
        } else if (config_.refine_param_.method ==
                   PPFEstimatorConfig::RefineMethod::PointToPlane) {
            // we use L1 loss for point to plane icp. See paper "Analysis of
            // Robust Functions for Registration Algorithms" for reference.
            auto loss =
                std::make_shared<open3d::pipelines::registration::L1Loss>();
            res = open3d::pipelines::registration::RegistrationICP(
                model_pcn, scene_pcn, max_correspondence_distance, init_pose,
                open3d::pipelines::registration::
                    TransformationEstimationPointToPlane(loss),
                criteria);
        }
#pragma omp critical
        {
            pose_list.emplace_back(max_votes_pose);
            if (config_.refine_param_.method !=
                PPFEstimatorConfig::RefineMethod::NoRefine) {
                pose_list[pose_list.size() - 1].UpdateByPose(
                    res.transformation_);
            }
        }
    }
}

void PPFEstimator::Impl::PoseAverage(const std::vector<Pose6D> &pose_list,
                                     const std::vector<int> &pose_idx,
                                     Pose6D &result_pose) {
    result_pose.corr_mi_ = -1;
    Eigen::Vector4d q_avg;
    Eigen::Matrix4d q_avg_mat = Eigen::Matrix4d::Zero();
    Eigen::Vector3d t_avg = Eigen::Vector3d::Zero();

    // Perform the final averaging
    const int curSize = (int)pose_idx.size();
    int cur_i;
    result_pose.num_votes_ = 0;

    for (int j = 0; j < curSize; j++) {
        cur_i = pose_idx[j];
        Eigen::Vector4d q_cv(pose_list[cur_i].q_);
        q_avg_mat += q_cv * q_cv.transpose();
        t_avg += Eigen::Vector3d(pose_list[cur_i].t_);
        result_pose.num_votes_ += pose_list[cur_i].num_votes_;
    }
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(q_avg_mat);
    q_avg = eigensolver.eigenvectors().col(3);
    t_avg *= 1.0 / double(curSize);
    result_pose.UpdateByQuat(q_avg, t_avg);
}

void PPFEstimator::Impl::DownSamplePCNormal(
    const open3d::geometry::PointCloud &pc,
    const std::shared_ptr<KDTree> &kdtree, const double radius,
    const size_t min_idx, open3d::geometry::PointCloud &pc_sample) {
    // This down sample method aims at sampling original point clouds by
    // distance interval, and hold the actual position of point itself. Because
    // voxel downsample method will change the position of points by averaging
    // the points inside a voxel.
    int cur_i, n_searched, near_i;
    double dist;
    const double search_r = radius * 2;
    const size_t n_pt = pc.points_.size();
    std::vector<bool> checked_flags(n_pt, false);
    std::queue<int> queue;
    queue.push((int)min_idx);
    std::vector<int> ret_indices;
    std::vector<double> out_dists_sqr;
    std::vector<size_t> selected_indices;
    while (!queue.empty()) {
        cur_i = queue.front();
        queue.pop();
        if (checked_flags[cur_i])
            continue;
        checked_flags[cur_i] = true;

        auto &p = pc.points_[cur_i];
        selected_indices.emplace_back(cur_i);
        n_searched =
            kdtree->SearchRadius(p, search_r, ret_indices, out_dists_sqr);

        for (int i = 0; i < n_searched; i++) {
            near_i = ret_indices[i];
            dist = sqrt(out_dists_sqr[i]);
            if (checked_flags[near_i])
                continue;
            if (dist < radius) {
                checked_flags[near_i] = true;
            } else {
                queue.push(near_i);
            }
        }
    }
    pc_sample = *pc.SelectByIndex(selected_indices);
}

void PPFEstimator::Impl::CalcModelNormalAndSampling(
    const PointCloudPtr &pc, const std::shared_ptr<KDTree> &kdtree,
    const double normal_r, const double step, const PointXYZ &view_pt,
    open3d::geometry::PointCloud &pc_sample) {
    pc_sample.Clear();
    const size_t n_pt = pc->points_.size();

    // calc normals
    if (!pc->HasNormals()) {
        pc->EstimateNormals(
            open3d::geometry::KDTreeSearchParamHybrid(normal_r, NORMAL_CALC_NN),
            false);
    }
    pc->NormalizeNormals();

    // calc nearest point respect to view point
    std::vector<int> ret_indices(1);
    std::vector<double> out_dists_sqr(1);
    const int n = kdtree->SearchKNN(view_pt, 1, ret_indices, out_dists_sqr);
    const PointXYZ nearest = pc->points_[ret_indices[0]];
    const int nearest_idx = ret_indices[0];
    const PointXYZ pt_2_view(view_pt(0) - nearest(0), view_pt(1) - nearest(1),
                             view_pt(2) - nearest(2));

    // modify normal based on orientation
    if (pt_2_view.transpose() * pc->normals_[nearest_idx] < 0)
        FlipNormal(pc->normals_[nearest_idx]);

    if (invert_normal_)
        FlipNormal(pc->normals_[nearest_idx]);

    // normal consistent
    std::vector<bool> calc_flags(n_pt, false);
    auto cmp = [](std::pair<size_t, double> &q0,
                  std::pair<size_t, double> &q1) {
        return q0.second > q1.second;
    };

    std::priority_queue<std::pair<size_t, double>,
                        std::vector<std::pair<size_t, double>>, decltype(cmp)>
        pq(cmp);

    int cur_i = nearest_idx, near_i, n_searched;
    std::pair<size_t, double> cur_pair(nearest_idx, 0);
    pq.push(cur_pair);
    std::vector<int> ret_indices2;
    std::vector<double> out_dists_sqr2;
    while (!pq.empty()) {
        cur_pair = pq.top();
        cur_i = cur_pair.first;
        pq.pop();
        if (calc_flags[cur_i])
            continue;
        const PointXYZ &temp = pc->points_[cur_i];
        n_searched =
            kdtree->SearchRadius(temp, normal_r, ret_indices2, out_dists_sqr2);

        for (size_t i = 0; i < n_searched; i++) {
            near_i = ret_indices2[i];
            if (!calc_flags[near_i]) {
                pq.push(std::make_pair(size_t(ret_indices2[i]),
                                       sqrt(out_dists_sqr2[i])));
            }
        }
        for (size_t i = 0; i < n_searched; i++) {
            near_i = ret_indices2[i];
            if (calc_flags[near_i]) {
                auto &cur_n = pc->normals_[cur_i];
                auto &near_n = pc->normals_[near_i];
                if (cur_n.dot(near_n) < 0)
                    cur_n *= -1;
                break;
            }
        }
        calc_flags[cur_i] = true;
    }

    DownSamplePCNormal(*pc, kdtree, step, nearest_idx, pc_sample);

    if (enable_edge_support_) {
        DownSamplePCNormal(*pc, kdtree, dist_step_dense_, nearest_idx,
                           dense_model_sample_);
        model_edge_ind_.clear();
        ExtractEdges(dense_model_sample_, normal_r, model_edge_ind_);
        misc3d::LogInfo("Extract {} edge points form model point clouds.",
                        model_edge_ind_.size());
    }
}

void PPFEstimator::Impl::ExtractEdges(const open3d::geometry::PointCloud &pc,
                                      double radius,
                                      std::vector<size_t> &edge_ind) {
    edge_ind = features::DetectBoundaryPoints(
        pc, open3d::geometry::KDTreeSearchParamHybrid(
                radius, config_.edge_param_.pts_num));
}

void PPFEstimator::Impl::CalcLocalMaximum(
    const size_t *accumulator, const size_t rows, const size_t cols,
    const size_t vote_threshold, const double relative_to_max,
    std::vector<std::vector<size_t>> &max_in_pair,
    std::vector<std::vector<int>> &neighbor_table) {
    max_in_pair.clear();
    size_t(*model_infos)[2] =
        new size_t[rows][2];  // 0: quantized angle, 1: votes number
    size_t final_idx = cols - 1;
    const size_t *acc_ptr = accumulator;
    for (int r = 0; r < rows; r++, acc_ptr += cols) {
        model_infos[r][1] = acc_ptr[cols - 1] + acc_ptr[0] + acc_ptr[1];
        model_infos[r][0] = 0;
        size_t temp;
        for (int c = 1; c < final_idx; c++) {
            temp = acc_ptr[c - 1] + acc_ptr[c] + acc_ptr[c + 1];
            if (temp > model_infos[r][1]) {
                model_infos[r][1] = temp;
                model_infos[r][0] = c;
            }
        }
        temp = acc_ptr[final_idx - 1] + acc_ptr[final_idx] + acc_ptr[0];
        if (temp > model_infos[r][1]) {
            model_infos[r][1] = temp;
            model_infos[r][0] = final_idx;
        }
    }
    size_t max_idx, max_votes = 0;
    for (int i = 0; i < rows; i++) {
        if (model_infos[i][1] > max_votes) {
            max_idx = i;
            max_votes = model_infos[i][1];
        }
    }
    if (max_votes < vote_threshold) {
        delete[] model_infos;
        return;
    }
    // 0: model index, 1: quantized angle, 2: votes number
    size_t n_neighbor;
    int *n_idx;
    const size_t votes_threshold = max_votes * relative_to_max;
    std::vector<bool> max_flags(rows, true);
    bool is_max;
    for (size_t i = 0; i < rows; i++) {
        if (model_infos[i][1] > votes_threshold && max_flags[i]) {
            is_max = true;
            n_neighbor = neighbor_table[i].size();
            n_idx = neighbor_table[i].data();
            for (int ni = 0; ni < n_neighbor; ni++, n_idx++) {
                if (model_infos[i][1] < model_infos[*n_idx][1]) {
                    is_max = false;
                } else {
                    max_flags[*n_idx] = false;
                }
            }
            max_flags[i] = is_max;
            if (is_max) {
                max_in_pair.push_back(
                    {i, model_infos[i][0], model_infos[i][1]});
            }
        }
    }
    delete[] model_infos;
}

void PPFEstimator::Impl::GenerateModelPCNeighbor(
    const open3d::geometry::PointCloud &pts,
    std::vector<std::vector<int>> &neighbor_table, const double radius) {
    const size_t pts_size = pts.points_.size();
    neighbor_table.resize(pts_size);
    KDTree kdtree(pts);

    int n_search;
#pragma omp parallel for
    for (int i = 0; i < pts_size; i++) {
        std::vector<int> ret_indices;
        std::vector<double> out_dists_sqr;
        const PointXYZ &temp = pts.points_[i];
        n_search =
            kdtree.SearchRadius(temp, radius, ret_indices, out_dists_sqr);
        neighbor_table[i] = ret_indices;
    }
}

void PPFEstimator::Impl::GenerateLUT() {
    int alpha_model_num = 2 * angle_num_ - 1;
    alpha_lut_.resize(alpha_model_num * alpha_model_num);
    int row_idx;
    for (int i = 0; i < alpha_model_num; i++) {
        row_idx = i * alpha_model_num;
        for (int j = 0; j < alpha_model_num; j++) {
            alpha_lut_[row_idx + j] =
                (i - j + alpha_model_num) % alpha_model_num;
        }
    }
    alpha_2_shift_lut_.resize(alpha_model_num);
    for (int i = 0; i < alpha_model_num; i++) {
        for (int j = 0; j < 3; j++) {
            alpha_2_shift_lut_[i][j] =
                (i + j - 1 + alpha_model_num) % alpha_model_num;
        }
    }
    alpha_shift_lut_.resize(angle_num_);
    int t;
    for (int i = 0; i < angle_num_; i++) {
        for (int j = 0; j < 3; j++) {
            t = i + j - 1;
            if (t < 0 || t >= angle_num_)
                t = -1;
            alpha_shift_lut_[i][j] = t;
        }
    }
    dist_shift_lut_.resize(dist_num_);
    for (int i = 0; i < dist_num_; i++) {
        for (int j = 0; j < 3; j++) {
            t = i + j - 1;
            if (t < 0 || t >= dist_num_)
                t = -1;
            dist_shift_lut_[i][j] = t;
        }
    }
}

PPFEstimator::PPFEstimator() {
    impl_ptr_ = std::make_unique<Impl>(PPFEstimatorConfig());
}

PPFEstimator::PPFEstimator(const PPFEstimatorConfig &config) {
    if (CheckConfig(config))
        impl_ptr_ = std::make_unique<Impl>(config);
}

PPFEstimator::~PPFEstimator() = default;

bool PPFEstimator::Train(const PointCloudPtr &pc) {
    if (impl_ptr_ != nullptr) {
        return impl_ptr_->Train(pc);
    } else {
        misc3d::LogError("PPF Estimator not is initialized successfully");
        return false;
    }
}

bool PPFEstimator::Estimate(const PointCloudPtr &pc,
                            std::vector<Pose6D> &results) {
    if (impl_ptr_ != nullptr) {
        return impl_ptr_->Estimate(pc, results);
    } else {
        misc3d::LogError("PPF Estimator not is initialized successfully");
        return false;
    }
}

bool PPFEstimator::Impl::SetConfig(const PPFEstimatorConfig &config) {
    config_ = config;
    dist_threshold_ = config.rel_dist_thresh_;
    // (calc_normal_relative_ = config.rel_sample_dist_ / 2) is usually a
    // good choise
    calc_normal_relative_ = config.training_param_.calc_normal_relative;
    enable_edge_support_ = (config.voting_param_.method ==
                            PPFEstimatorConfig::VotingMode::EdgePoints)
                               ? true
                               : false;
    invert_normal_ = config.training_param_.invert_model_normal;
    return true;
}

bool PPFEstimator::SetConfig(const PPFEstimatorConfig &config) {
    if (CheckConfig(config)) {
        impl_ptr_->SetConfig(config);
        return true;
    } else {
        return false;
    }
}

double PPFEstimator::GetModelDiameter() {
    return impl_ptr_->diameter_;
}

std::vector<Pose6D> PPFEstimator::GetPose() {
    return impl_ptr_->pose_list_;
}

open3d::geometry::PointCloud PPFEstimator::GetSampledModel() {
    return impl_ptr_->model_sample_;
}

open3d::geometry::PointCloud PPFEstimator::GetSampledScene() {
    return impl_ptr_->scene_sample_;
}

open3d::geometry::PointCloud PPFEstimator::GetModelEdges() {
    open3d::geometry::PointCloud out;
    if (impl_ptr_->enable_edge_support_) {
        out = *impl_ptr_->dense_model_sample_.SelectByIndex(
            impl_ptr_->model_edge_ind_);
    } else {
        misc3d::LogWarning(
            "Can not get model edge points due to edge support is not "
            "enabled.");
    }

    return out;
}

open3d::geometry::PointCloud PPFEstimator::GetSceneEdges() {
    open3d::geometry::PointCloud out;
    if (impl_ptr_->enable_edge_support_) {
        out = *impl_ptr_->dense_scene_sample_.SelectByIndex(
            impl_ptr_->scene_edge_ind_);
    } else {
        misc3d::LogWarning(
            "Can not get scene edge points due to edge support is not "
            "enabled.");
    }

    return out;
}

// set default parameters
PPFEstimatorConfig::PPFEstimatorConfig() {
    training_param_ = {false, 0.05, 0.025, 0.01};
    ref_param_ = {ReferencePointSelection::Random, 0.6};
    voting_param_ = {VotingMode::SampledPoints, true, Deg2Rad<double>(12),
                     1.0 / 3, Deg2Rad<double>(30)};
    edge_param_ = {20};
    refine_param_ = {RefineMethod::PointToPlane, 5};

    rel_dist_thresh_ = 0.05;
    rel_angle_thresh_ = Deg2Rad<double>(12);
    score_thresh_ = 0.6;
    num_result_ = 10;
    object_id_ = 0;
}

PPFEstimatorConfig::~PPFEstimatorConfig() = default;

bool PPFEstimator::CheckConfig(const PPFEstimatorConfig &config) {
    if (config.training_param_.rel_dense_sample_dist >=
        config.training_param_.rel_sample_dist) {
        misc3d::LogError(
            "Dense_sample_dist should be smaller than sample_dist.");
        return false;
    }

    return true;
}

}  // namespace pose_estimation
}  // namespace misc3d