#include "../include/mpc.hpp"
#include <cppad/ipopt/solve.hpp>
#include <Eigen/Core>
#include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/transform_datatypes.h>

#include <chrono>
#include <iostream>
using namespace std;

MPC::MPC(double _horizonDurationForward, double _horizonSamplingForward,
         double _horizonDurationRotate, double _horizonSamplingRotate,
         const Constraints &_constraints, const CostWeights &_weights,
         const double _goalArea, const std::string &_solverOptions)
    : horizonSamplingForward(_horizonSamplingForward), horizonDurationForward(_horizonDurationForward),
      horizonSamplingRotate(_horizonSamplingRotate), horizonDurationRotate(_horizonDurationRotate),
      constraints(_constraints), weights(_weights),
      goalArea(_goalArea), solverOptions(_solverOptions), tfThread(&MPC::tfThreadFcn, this)
{
}

MPC::~MPC()
{
    stopTfThread = true;
    tfThread.join();
}

void MPC::tfThreadFcn()
{
    auto previousTfTime = std::chrono::steady_clock::now();

    while (!stopTfThread)
    {
        auto now = std::chrono::steady_clock::now();

        // Check for tf timeouts to stop the controller.
        auto tfElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - previousTfTime).count();
        if (tfElapsed > 3000)
            tfReceived = false;

        // Get location of the robot on local map.
        bool ok = false;
        geometry_msgs::TransformStamped robotTransform = getTransform("statek/earth", "statek/base_footprint", ok);

        if (ok)
        {
            tfReceived = true; // Tf received so allow MPC to work.
            previousTfTime = std::chrono::steady_clock::now();

            // Convert orientation to tf quaternion.
            tf::Quaternion quat;
            tf::quaternionMsgToTF(robotTransform.transform.rotation, quat);

            // Convert orientation to RPY angles.
            double temp1, temp2, theta;
            tf::Matrix3x3(quat).getRPY(temp1, temp2, theta);
            theta = wrapToPi(theta);

            // Set current state.
            const std::lock_guard<std::mutex> lck(stateMtx);
            state = {robotTransform.transform.translation.x,
                     robotTransform.transform.translation.y,
                     theta, 0, 0, -1};

            // Get cross track error and orientation error.
            findCteOe(state, 0);
        }
    }
}

double MPC::wrapToPi(double angle)
{
    // Whiles because fmod works strange?
    if (angle > M_PI)
        angle -= M_PI;
    if (angle < -M_PI)
        angle += M_PI;
    if (angle == -M_PI)
        angle = M_PI;

    return angle;
}

double MPC::distancePointLine(double x0, double y0, double x1, double y1, double x2, double y2)
{
    return fabs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

void MPC::findCteOe(State &_state, int lastIdx) const
{
    int bestIdx = -1;
    _state.cte = std::numeric_limits<double>::max();

    for (int i = lastIdx; i < path.size(); i++)
    {
        double currentCte = distancePointLine(_state.x, _state.y, path[i].x0, path[i].y0, path[i].x1, path[i].y1);
        if (currentCte < _state.cte)
        {
            _state.cte = currentCte;
            bestIdx = i;
        }
    }

    if (bestIdx >= 0)
    {
        double distanceToEnd = sqrt(pow(_state.x - path[bestIdx].x1, 2) + pow(_state.y - path[bestIdx].y1, 2));

        // If distance of the vehicle and end of best segment is less than some limiting value
        // then we'd rather care for next segment if there is one.
        if (distanceToEnd < goalArea &&
            bestIdx < path.size() - 1)
        {
            bestIdx++;
            _state.cte = distancePointLine(_state.x, _state.y, path[bestIdx].x0, path[bestIdx].y0, path[bestIdx].x1, path[bestIdx].y1);
        }

        _state.oe = wrapToPi(path[bestIdx].angle - _state.yaw);
    }

    _state.cteIdx = bestIdx;
}

geometry_msgs::TransformStamped MPC::getTransform(const std::string &targetFrame, const std::string &sourceFrame, bool &ok)
{
    static tf2_ros::Buffer tfBuffer;
    static tf2_ros::TransformListener transformListener(tfBuffer);

    geometry_msgs::TransformStamped result;

    if (targetFrame == "" || sourceFrame == "")
    {
        ok = false;
        return result;
    }

    // Fixes extrapolation into the future in Rviz if run remotely.
    ros::Time now = ros::Time::now();
    ok = tfBuffer.canTransform(targetFrame, sourceFrame, now, ros::Duration(10.0));
    if (!ok)
        return result;

    ok = true;
    try
    {
        result = tfBuffer.lookupTransform(targetFrame, sourceFrame, now);
    }
    catch (tf::TransformException ex)
    {
        ok = false;
    }

    return result;
}

MPC::Control MPC::update()
{
    auto startFcn = chrono::steady_clock::now();

    if (!tfReceived || !path.size())
        return {0, 0, {}, true};

    {
        const std::lock_guard<std::mutex> lckOe(stateMtx);

        // Calculace current prediction horizon depending on oe.
        // Use simple linear equation to do that.
        horizonSampling = abs(state.oe) * (horizonSamplingRotate - horizonSamplingForward) / M_PI + horizonSamplingForward;
        horizonDuration = abs(state.oe) * (horizonDurationRotate - horizonDurationForward) / M_PI + horizonDurationForward;
        N = horizonDuration / horizonSampling;
    }

    int numVars = numInputs * (N - 1);
    angularVelocityStart = linearVelocityStart + N - 1;

    // Initial state of variables.
    Dvector vars(numVars);
    for (int i = 0; i < numVars; i++)
    {
        vars[i] = 0;
    }

    // Constraints for variables.
    Dvector varsLowerConstraints(numVars);
    Dvector varsUpperConstraints(numVars);
    for (int i = linearVelocityStart; i < angularVelocityStart; i++)
    {
        varsLowerConstraints[i] = constraints.linearMin;
        varsUpperConstraints[i] = constraints.linearMax;
    }
    for (int i = angularVelocityStart; i < numVars; i++)
    {
        varsLowerConstraints[i] = constraints.angularMin;
        varsUpperConstraints[i] = constraints.angularMax;
    }

    // Constraints for constraint equations g(x).
    Dvector stateLowerConstraints(0);
    Dvector stateUpperConstraints(0);

    // MPC evaluation.
    FG_eval evalFcn(this);
    CppAD::ipopt::solve_result<Dvector> solution;

    const std::lock_guard<std::mutex> lckOpti(stateMtx);
    try
    {
        CppAD::ipopt::solve<Dvector, FG_eval>(
            solverOptions,
            vars, varsLowerConstraints, varsUpperConstraints,
            varsLowerConstraints, varsUpperConstraints,
            evalFcn, solution);
    }
    catch (...)
    {
        return {0, 0, {}, true};
    }

    /*if (solution.status != CppAD::ipopt::solve_result<Dvector>::success)
    {
        return {0, 0, {}, true};
    }*/

    Control control;
    control.success = true;

    // Apply first set of controls.
    control.linearVelocity = solution.x[linearVelocityStart];
    control.angularVelocity = solution.x[angularVelocityStart];

    // Copy state trajectory.
    control.stateTrajectory = evalFcn.evalControl(solution.x, state);

    auto endFcn = chrono::steady_clock::now();
    //ROS_WARN("MPC update elapsed time in milliseconds: %ldms.\n---", chrono::duration_cast<chrono::milliseconds>(endFcn - startFcn).count());

    return control;
}

void MPC::onNewPath(const nav_msgs::Path::ConstPtr &pathMsg)
{
    if (!pathMsg->poses.size())
        return;

    // If previous and current paths were not a straight line
    // but a bunch of segments, then check whether it is necessary to update it.
    // This prevents vehicle from being stuck in front of an obstacle
    // when map changes rapidly fron one side of this obstacle to another.
    if (path.size() > 1 && pathMsg->poses.size() > 2)
    {
        double distanceToGoal;
        double distanceTravelled;
        {
            const std::lock_guard<std::mutex> lckOpti(stateMtx);
            distanceToGoal = sqrt(pow(path[0].x1 - state.x, 2) + pow(path[0].y1 - state.y, 2));
            distanceTravelled = sqrt(pow(onMapState.x - state.x, 2) + pow(onMapState.y - state.y, 2));
        }

        // Ignore path if the vehicle haven't achieved subgoal
        // or travelled a bit or some time passed.
        if (distanceToGoal > goalArea || distanceTravelled < 0.2)
            return;
    }

    path.clear();

    // Translate path message into Path data type.
    for (int i = 0; i < pathMsg->poses.size() - 1; i++)
    {
        PathSegment segment;

        segment.x0 = pathMsg->poses[i].pose.position.x;
        segment.y0 = pathMsg->poses[i].pose.position.y;

        segment.x1 = pathMsg->poses[i + 1].pose.position.x;
        segment.y1 = pathMsg->poses[i + 1].pose.position.y;

        segment.angle = wrapToPi(atan2(segment.y1 - segment.y0, segment.x1 - segment.x0));

        path.push_back(segment);
    }

    const std::lock_guard<std::mutex> lckOpti(stateMtx);
    onMapState = state;
}

void MPC::onNewInputs(const geometry_msgs::TwistStamped::ConstPtr &currentInputs)
{
    inputs.linearVelocity = currentInputs->twist.linear.x;
    inputs.angularVelocity = currentInputs->twist.angular.z;
}


#include "maze/builder.h"
#include <algorithm>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <chrono>
#include <functional>  // std::reference_wrapper
#include "maze/utils.h"
#include "maze/base.h"

class Room;  // forward declaration
using namespace utils;

namespace maze {
typedef std::vector<std::reference_wrapper<Room>> Rooms;
typedef std::reference_wrapper<Room> RoomRef;

// Default constructor
Builder::Builder(){};

// Constructor
Builder::Builder(const Maze& maze) : _maze{maze} {
  Position start{random_integer(1, _maze.columns()),
                 random_integer(1, _maze.rows())};
  _path.push_back(std::move(start));
  _visited_positions.push_back(std::move(start));
}

boost::optional<Position> Builder::current_position() const {
  boost::optional<Position> last = _visited_positions.rbegin()[0];
  if (_visited_positions.size() >= 1 && last != boost::none) return last;
  return boost::none;
}

boost::optional<Position> Builder::previous_position() const {
  boost::optional<Position> second_to_last = _visited_positions.rbegin()[1];
  if (_visited_positions.size() >= 2 && second_to_last != boost::none)
    return second_to_last;
  return boost::none;
}

void Builder::go_back_to_previous_visited_room() {
  if (!_visited_positions.empty()) _visited_positions.pop_back();
}

boost::optional<Room*> Builder::room(const Position& position) const {
  return _maze.find_room(position);
}

boost::optional<Room*> Builder::current_room() {
  auto current_position_ = current_position();
  if (current_position_ != boost::none) return room(*current_position_);
  return boost::none;
}

boost::optional<const Position> Builder::next_position(
    const Direction& direction, const Position& position) const {
  int next_position_x, next_position_y;
  bool out_of_bounds{false};
  switch (direction) {
    case Direction::LEFT: {
      if (position.x - 1 >= 1) {
        next_position_x = position.x - 1;
        next_position_y = position.y;
      } else {
        out_of_bounds = true;
      }
      break;
    }
    case Direction::RIGHT: {
      if (position.x + 1 <= _maze.columns()) {
        next_position_x = position.x + 1;
        next_position_y = position.y;
      } else {
        out_of_bounds = true;
      }
      break;
    }
    case Direction::DOWN: {
      if (position.y + 1 <= _maze.rows()) {
        next_position_x = position.x;
        next_position_y = position.y + 1;

      } else {
        out_of_bounds = true;
      }
      break;
    }
    case Direction::UP: {
      if (position.y - 1 >= 1) {
        next_position_x = position.x;
        next_position_y = position.y - 1;
      } else {
        out_of_bounds = true;
      }
      break;
    }
    default: {
      return boost::none;
      break;
    }
  }
  if (out_of_bounds) return boost::none;
  Position next_position{next_position_x, next_position_y};
  return next_position;
}

boost::optional<Direction> Builder::determine_direction(
    const Room& next_room) const {
  boost::optional<Direction> direction_to_next_room;
  auto direction_iter = std::find_if(
      DIRECTIONS.begin(), DIRECTIONS.end(),
      [ this, nr = next_room ](Direction direction) mutable->bool {
        auto current_position_ = current_position();
        auto next_position_ = (current_position_ != boost::none)
                                  ? next_position(direction, *current_position_)
                                  : boost::none;
        auto room_to_direction = (next_position_ != boost::none)
                                     ? room(*next_position_)
                                     : boost::none;
        return (room_to_direction != boost::none &&
                (**room_to_direction).position() == nr.position());
      });
  if (direction_iter != DIRECTIONS.end()) return *direction_iter;
  return boost::none;
}

Rooms Builder::valid_rooms_to_build() const {
  Rooms valid_rooms;
  for (auto direction : DIRECTIONS) {
    auto cp_ptr = current_position();
    auto np = next_position(direction, *cp_ptr);
    auto room_to_direction = (np == boost::none || room(*np) == boost::none)
                                 ? boost::none
                                 : room(*np);
    if (room_to_direction != boost::none && !(**room_to_direction).visited()) {
      valid_rooms.push_back(**room_to_direction);
    }
  }

  auto pp = previous_position();
  if (pp != boost::none) {
    auto previous_room = room(*pp);
    valid_rooms.erase(
        std::remove_if(std::begin(valid_rooms), std::end(valid_rooms),
                       [&previous_room](RoomRef r) {
                         return r.get().position() ==
                                (**previous_room).position();
                       }),
        std::end(valid_rooms));
  };
  return valid_rooms;
};

void Builder::build_room(Room& a_room, const Direction& exit_to_free) {
  a_room.available_exits().push_back(exit_to_free);
  a_room.visits_from().push_back(exit_to_free);
}

void Builder::build_maze() {
  printf("Started building maze of %lu rooms.\n", _maze.rooms().size());
  auto start = std::chrono::high_resolution_clock::now();
  while (!_maze.all_rooms_visited()) {
    auto valid_rooms_to_build_ = valid_rooms_to_build();
    auto current_room_ = current_room();
    if (!valid_rooms_to_build_.empty()) {
      Room& next_room = random_element(valid_rooms_to_build_).get();
      auto direction = determine_direction(next_room);
      build_room(**current_room_, *direction);
      _path.push_back(next_room.position());
      _visited_positions.push_back(next_room.position());
      build_room(next_room, maze::opposite_direction.at(*direction));
    } else {
      go_back_to_previous_visited_room();
      _path.push_back((**current_room()).position());
    }
  }
  auto end = std::chrono::high_resolution_clock::now();
  auto execution_time =
      std::chrono::duration_cast<std::chrono::milliseconds>(end - start)
          .count();
  printf("Maze built in %lu steps, %lu.%lu seconds. \n", _path.size(),
         execution_time / 1000, execution_time % 1000);
}

const Maze& Builder::maze() const { return _maze; }
const std::vector<Position>& Builder::path() const { return _path; }

}  // namespace maze


#include "Config.h"

#include <algorithm>
#include <bitset>

#include <boost/program_options.hpp>
#include <boost/log/trivial.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/smart_ptr/make_shared.hpp>

namespace org::openkilda {

    namespace po = boost::program_options;

    class InvalidCmd : public std::exception {
    };

    class Config_impl : public Config {
    private:
        Config_impl(Config_impl const &) = delete;

        Config_impl &operator=(Config_impl const &) = delete;

        uint32_t core_mask;
        uint32_t master_lcore;
        uint32_t mbuf_pool_size_per_device;
        uint32_t process_queue_size;
        uint32_t first_stats_port;
        uint32_t control_port;
        bool debug;

    public:

        Config_impl() = default;

        void fill_from_cmd(int argc, char **argv) {

            po::variables_map vm = parse_cmd(argc, argv);

            process_core_mask(vm);

            process_master_lcore(vm);

            process_mbuf_pool_size_per_device(vm);

            process_process_queue_size(vm);

            process_first_stats_port(vm);

            process_control_port(vm);

            process_debug(vm);
        }

        uint32_t get_core_mask() const override {
            return core_mask;
        }

        uint32_t get_master_lcore() const override {
            return master_lcore;
        }

        uint32_t get_mbuf_pool_size_per_device() const override {
            return mbuf_pool_size_per_device;
        }

        uint32_t get_process_queue_size() const override {
            return process_queue_size;
        }

        uint32_t get_first_stats_port() const override {
            return first_stats_port;
        }

        uint32_t get_control_port() const override {
            return control_port;
        }

        virtual bool is_debug() const override {
            return debug;
        }

        constexpr static const char *arg_core = "c";
        constexpr static const char *arg_master_core = "master-lcore";
        constexpr static const char *arg_mbuf_pool_size_per_device = "kilda-mbuf-pool-size-per-device";
        constexpr static const char *arg_process_queue_size = "process-queue-size";
        constexpr static const char *arg_first_stats_port = "first-stats-port";
        constexpr static const char *arg_control_port = "control-port";
        constexpr static const char *arg_debug = "debug";

    private:

        po::variables_map parse_cmd(uint32_t argc, char **argv) {
            // remove "--" from argv
            int argc_actual = argc;
            char **argv_actual = argv;

            std::vector<char *> argv_filtered;

            std::copy_if(argv_actual, argv_actual + argc_actual,
                         std::back_inserter(argv_filtered), [](char *s) { return std::string(s) != "--"; });

            if (argc != argv_filtered.size()) {
                argv_actual = argv_filtered.data();
                argc_actual = argv_filtered.size();
            }

            // end of filtering

            po::variables_map vm;
            po::store(
                    po::command_line_parser(argc_actual, argv_actual)
                            .options(make_description())
                            .style(
                                    po::command_line_style::unix_style
                                    | po::command_line_style::allow_long_disguise)
                            .allow_unregistered()
                            .run(),
                    vm);

            return vm;
        }

        po::options_description make_description() {
            po::options_description desc;
            desc.add_options()
                    (arg_core, po::value<std::string>(), "Set the hexadecimal bitmask of the cores to run on.")
                    (arg_master_core, po::value<int>()->default_value(0), "Core ID that is used as master.")
                    (arg_mbuf_pool_size_per_device, po::value<int>()->default_value(4095),
                     "The mbuf pool size each DpdkDevice will have. Must be 2^N-1")
                    (arg_process_queue_size, po::value<int>()->default_value(32768),
                     "Size of queue from read thread and send thread Must be power of 2")
                    (arg_first_stats_port, po::value<int>()->default_value(5556), "First stats port for zmq")
                    (arg_control_port, po::value<int>()->default_value(5555), "Control port for zmq")
                    (arg_debug, po::bool_switch());
            return desc;
        }

        void process_core_mask(const po::variables_map &vm) {

            if (vm.count("c")) {
                std::string core_mask_option = vm["c"].as<std::string>();
                BOOST_LOG_TRIVIAL(info) << "core mask: " << core_mask_option;
                core_mask = std::stoul(core_mask_option, 0, 16);

                if (core_mask == 0) {
                    BOOST_LOG_TRIVIAL(fatal) << "core mask must be greater than 0";
                    throw InvalidCmd{};
                }

                std::bitset<128> cores(core_mask);
                for (size_t i = 0; i < cores.size(); ++i) {
                    if (cores.test(i)) {
                        BOOST_LOG_TRIVIAL(info) << "core " << i << " enabled for dpkd";
                    }
                }

            } else {
                BOOST_LOG_TRIVIAL(fatal) << "core mask must be set";
                throw InvalidCmd{};
            }
        }

        void process_master_lcore(const po::variables_map &vm) {
            master_lcore = vm[arg_master_core].as<int>();
            BOOST_LOG_TRIVIAL(info) << "master-lcore " << master_lcore;
        }

        void process_mbuf_pool_size_per_device(const po::variables_map &vm) {
            mbuf_pool_size_per_device = vm[arg_mbuf_pool_size_per_device].as<int>();
            BOOST_LOG_TRIVIAL(info) << "mbuf-pool-size-per-device " << mbuf_pool_size_per_device;
        }

        void process_process_queue_size(const po::variables_map &vm) {
            process_queue_size = vm[arg_process_queue_size].as<int>();
            BOOST_LOG_TRIVIAL(info) << "process-queue-size " << process_queue_size;
        }

        void process_first_stats_port(const po::variables_map &vm) {
            first_stats_port = vm[arg_first_stats_port].as<int>();
            BOOST_LOG_TRIVIAL(info) << "first-stats-port " << first_stats_port;
        }

        void process_control_port(const po::variables_map &vm) {
            control_port = vm[arg_control_port].as<int>();
            BOOST_LOG_TRIVIAL(info) << "control-port " << control_port;
        }

        void process_debug(const po::variables_map &vm) {
            debug = vm[arg_debug].as<bool>();
            BOOST_LOG_TRIVIAL(info) << "debug " << debug;
        }

    };

    int create_config_from_cmd(int argc, char **argv, Config::ptr &config) {
        auto p = boost::make_shared<Config_impl>();
        try {
            p->fill_from_cmd(argc, argv);
        } catch (InvalidCmd _) {
            return 1;
        }
        config = std::move(p);
        return 0;
    }

}


// $Id$
//
//  Copyright (C) 2013 Paolo Tosco
//
//  Copyright (C) 2004-2006 Rational Discovery LLC
//
//   @@ All Rights Reserved @@
//  This file is part of the RDKit.
//  The contents are covered by the terms of the BSD license
//  which is included in the file license.txt, found at the root
//  of the RDKit source tree.
//
#include "TorsionAngle.h"
#include "TorsionConstraint.h"
#include "Params.h"
#include <cmath>

#include <RDGeneral/BoostStartInclude.h>
#include <boost/math/special_functions/round.hpp>
#include <RDGeneral/BoostEndInclude.h>

#include <ForceField/ForceField.h>
#include <RDGeneral/Invariant.h>

namespace ForceFields {
namespace MMFF {
void _pretreatDihedrals(double &minDihedralDeg, double &maxDihedralDeg) {
  if (minDihedralDeg < 0.0) minDihedralDeg += 360.0;
  if (maxDihedralDeg < 0.0) maxDihedralDeg += 360.0;
  minDihedralDeg = fmod(minDihedralDeg, 360.0);
  maxDihedralDeg = fmod(maxDihedralDeg, 360.0);
  if (maxDihedralDeg < minDihedralDeg) maxDihedralDeg += 360.0;
}

TorsionConstraintContrib::TorsionConstraintContrib(
    ForceField *owner, unsigned int idx1, unsigned int idx2, unsigned int idx3,
    unsigned int idx4, double minDihedralDeg, double maxDihedralDeg,
    double forceConst) {
  PRECONDITION(owner, "bad owner");
  URANGE_CHECK(idx1, owner->positions().size());
  URANGE_CHECK(idx2, owner->positions().size());
  URANGE_CHECK(idx3, owner->positions().size());
  URANGE_CHECK(idx4, owner->positions().size());
  PRECONDITION((!(maxDihedralDeg < minDihedralDeg)) &&
                   ((maxDihedralDeg - minDihedralDeg) < 360.0),
               "bad bounds");
  _pretreatDihedrals(minDihedralDeg, maxDihedralDeg);

  dp_forceField = owner;
  d_at1Idx = idx1;
  d_at2Idx = idx2;
  d_at3Idx = idx3;
  d_at4Idx = idx4;
  d_minDihedralDeg = minDihedralDeg;
  d_maxDihedralDeg = maxDihedralDeg;
  d_forceConstant = forceConst;
}

TorsionConstraintContrib::TorsionConstraintContrib(
    ForceField *owner, unsigned int idx1, unsigned int idx2, unsigned int idx3,
    unsigned int idx4, bool relative, double minDihedralDeg,
    double maxDihedralDeg, double forceConst) {
  PRECONDITION(owner, "bad owner");
  const RDGeom::PointPtrVect &pos = owner->positions();
  URANGE_CHECK(idx1, pos.size());
  URANGE_CHECK(idx2, pos.size());
  URANGE_CHECK(idx3, pos.size());
  URANGE_CHECK(idx4, pos.size());
  PRECONDITION((!(maxDihedralDeg < minDihedralDeg)) &&
                   ((maxDihedralDeg - minDihedralDeg) < 360.0),
               "bad bounds");

  double dihedral = 0.0;
  if (relative) {
    RDGeom::Point3D p1 = *((RDGeom::Point3D *)pos[idx1]);
    RDGeom::Point3D p2 = *((RDGeom::Point3D *)pos[idx2]);
    RDGeom::Point3D p3 = *((RDGeom::Point3D *)pos[idx3]);
    RDGeom::Point3D p4 = *((RDGeom::Point3D *)pos[idx4]);
    RDGeom::Point3D r12 = p2 - p1;
    RDGeom::Point3D r23 = p3 - p2;
    RDGeom::Point3D r34 = p4 - p3;

    RDGeom::Point3D n123 = r12.crossProduct(r23);
    double nIJKSqLength = n123.lengthSq();
    RDGeom::Point3D n234 = r23.crossProduct(r34);
    double nJKLSqLength = n234.lengthSq();
    RDGeom::Point3D m = n123.crossProduct(r23);
    // we want a signed dihedral, that's why we use atan2 instead of acos
    dihedral =
        RAD2DEG *
        (-atan2(m.dotProduct(n234) / sqrt(nJKLSqLength * m.lengthSq()),
                n123.dotProduct(n234) / sqrt(nIJKSqLength * nJKLSqLength)));
  }
  dp_forceField = owner;
  d_at1Idx = idx1;
  d_at2Idx = idx2;
  d_at3Idx = idx3;
  d_at4Idx = idx4;
  minDihedralDeg += dihedral;
  maxDihedralDeg += dihedral;
  _pretreatDihedrals(minDihedralDeg, maxDihedralDeg);
  d_minDihedralDeg = minDihedralDeg;
  d_maxDihedralDeg = maxDihedralDeg;
  d_forceConstant = forceConst;
}

double TorsionConstraintContrib::getEnergy(double *pos) const {
  PRECONDITION(dp_forceField, "no owner");
  PRECONDITION(pos, "bad vector");

  RDGeom::Point3D p1(pos[3 * d_at1Idx], pos[3 * d_at1Idx + 1],
                     pos[3 * d_at1Idx + 2]);
  RDGeom::Point3D p2(pos[3 * d_at2Idx], pos[3 * d_at2Idx + 1],
                     pos[3 * d_at2Idx + 2]);
  RDGeom::Point3D p3(pos[3 * d_at3Idx], pos[3 * d_at3Idx + 1],
                     pos[3 * d_at3Idx + 2]);
  RDGeom::Point3D p4(pos[3 * d_at4Idx], pos[3 * d_at4Idx + 1],
                     pos[3 * d_at4Idx + 2]);

  RDGeom::Point3D r1 = p1 - p2;
  RDGeom::Point3D r2 = p3 - p2;
  RDGeom::Point3D r3 = p2 - p3;
  RDGeom::Point3D r4 = p4 - p3;
  RDGeom::Point3D t1 = r1.crossProduct(r2);
  RDGeom::Point3D t2 = r3.crossProduct(r4);
  double d1 = std::max(t1.length(), 0.0);
  double d2 = std::max(t2.length(), 0.0);
  t1 /= d1;
  t2 /= d2;

  RDGeom::Point3D n123 = (-r1).crossProduct(r2);
  double n123SqLength = n123.lengthSq();
  RDGeom::Point3D n234 = r2.crossProduct(r4);
  double n234SqLength = n234.lengthSq();
  RDGeom::Point3D m = n123.crossProduct(r2);
  // we want a signed dihedral, that's why we use atan2 instead of acos
  double dihedral =
      RAD2DEG *
      (-atan2(m.dotProduct(n234) / sqrt(n234SqLength * m.lengthSq()),
              n123.dotProduct(n234) / sqrt(n123SqLength * n234SqLength)));
  if (dihedral < 0.0) dihedral += 360.0;
  double dihedralTerm = 0.0;
  if (dihedral < d_minDihedralDeg) {
    dihedralTerm = dihedral - d_minDihedralDeg;
  } else if (dihedral > d_maxDihedralDeg) {
    dihedralTerm = dihedral - d_maxDihedralDeg;
  }
  double const c = 0.5 * DEG2RAD * DEG2RAD;
  double res = c * d_forceConstant * dihedralTerm * dihedralTerm;

  return res;
}

void TorsionConstraintContrib::getGrad(double *pos, double *grad) const {
  PRECONDITION(dp_forceField, "no owner");
  PRECONDITION(pos, "bad vector");
  PRECONDITION(grad, "bad vector");

  RDGeom::Point3D p1(pos[3 * d_at1Idx], pos[3 * d_at1Idx + 1],
                     pos[3 * d_at1Idx + 2]);
  RDGeom::Point3D p2(pos[3 * d_at2Idx], pos[3 * d_at2Idx + 1],
                     pos[3 * d_at2Idx + 2]);
  RDGeom::Point3D p3(pos[3 * d_at3Idx], pos[3 * d_at3Idx + 1],
                     pos[3 * d_at3Idx + 2]);
  RDGeom::Point3D p4(pos[3 * d_at4Idx], pos[3 * d_at4Idx + 1],
                     pos[3 * d_at4Idx + 2]);
  double *g[4] = {&(grad[3 * d_at1Idx]), &(grad[3 * d_at2Idx]),
                  &(grad[3 * d_at3Idx]), &(grad[3 * d_at4Idx])};

  RDGeom::Point3D r[4] = {p1 - p2, p3 - p2, p2 - p3, p4 - p3};
  RDGeom::Point3D t[2] = {r[0].crossProduct(r[1]), r[2].crossProduct(r[3])};
  double d[2] = {t[0].length(), t[1].length()};
  if (isDoubleZero(d[0]) || isDoubleZero(d[1])) {
    return;
  }
  t[0] /= d[0];
  t[1] /= d[1];
  double cosPhi = t[0].dotProduct(t[1]);
  clipToOne(cosPhi);
  double sinPhiSq = 1.0 - cosPhi * cosPhi;
  double sinPhi = ((sinPhiSq > 0.0) ? sqrt(sinPhiSq) : 0.0);
  // dE/dPhi is independent of cartesians:

  RDGeom::Point3D n123 = (-r[0]).crossProduct(r[1]);
  double n123SqLength = n123.lengthSq();
  RDGeom::Point3D n234 = r[1].crossProduct(r[3]);
  double n234SqLength = n234.lengthSq();
  RDGeom::Point3D m = n123.crossProduct(r[1]);
  // we want a signed dihedral, that's why we use atan2 instead of acos
  double dihedral =
      RAD2DEG *
      (-atan2(m.dotProduct(n234) / sqrt(n234SqLength * m.lengthSq()),
              n123.dotProduct(n234) / sqrt(n123SqLength * n234SqLength)));
  if (dihedral < 0.0) dihedral += 360.0;
  // double dihedral = RAD2DEG * acos(cosPhi);
  double dihedralTerm = 0.0;
  if (dihedral < d_minDihedralDeg) {
    dihedralTerm = dihedral - d_minDihedralDeg;
  } else if (dihedral > d_maxDihedralDeg) {
    dihedralTerm = dihedral - d_maxDihedralDeg;
  }
  if (dihedral > 180.0) dihedralTerm = -dihedralTerm;
  double dE_dPhi = DEG2RAD * d_forceConstant * dihedralTerm;

  // FIX: use a tolerance here
  // this is hacky, but it's per the
  // recommendation from Niketic and Rasmussen:
  double sinTerm =
      -dE_dPhi * (isDoubleZero(sinPhi) ? (1.0 / cosPhi) : (1.0 / sinPhi));
  Utils::calcTorsionGrad(r, t, d, g, sinTerm, cosPhi);
}
}
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include <sstream> // stringstream

// evil hack to allow testing of private and protected data
#define private public
#define protected public

#include "niflib.h"
#include "obj/NiNode.h"
#include "obj/NiTriShape.h"
#include "obj/NiTriShapeData.h"

using namespace Niflib;
using namespace std;

BOOST_AUTO_TEST_SUITE(trishape_test_suite)

BOOST_AUTO_TEST_CASE(trishape_match_test)
{
  stringstream ss;
  stringstream ss2;

  // create a simple nif tree with a trishape
  NiNodeRef root = new NiNode;
  NiTriShapeRef shape = new NiTriShape;
  NiTriShapeDataRef data = new NiTriShapeData;
  // set hierarchy
  shape->SetData(data);
  root->AddChild(DynamicCast<NiAVObject>(shape));
  // add triangles and vertices to the geometry
  {
    vector<Vector3> verts;
    vector<Vector3> norms;
    vector<Triangle> tris;
    // 0
    verts.push_back(Vector3(0, 0, 0));
    norms.push_back(Vector3(0, 0, 1));
    // 1
    verts.push_back(Vector3(0, 1, 0));
    norms.push_back(Vector3(0, 0, 1));
    // 2
    verts.push_back(Vector3(0, 0, 1));
    norms.push_back(Vector3(0, 1, 0));
    // 3 == 0
    verts.push_back(Vector3(0, 0, 0));
    norms.push_back(Vector3(0, 0, 1));
    // 4
    verts.push_back(Vector3(0, 1, 0));
    norms.push_back(Vector3(0, 1, 0));
    // 5 == 0
    verts.push_back(Vector3(0, 0, 0));
    norms.push_back(Vector3(0, 0, 1));
    // 6 == 2
    verts.push_back(Vector3(0, 0, 1));
    norms.push_back(Vector3(0, 1, 0));
    data->SetVertices(verts);
    data->SetNormals(norms);
  }
  // generate match groups and check them
  BOOST_CHECK_NO_THROW(data->DoMatchDetection());
  BOOST_CHECK_EQUAL(data->matchGroups.size(), 2);
  BOOST_CHECK_EQUAL(data->matchGroups[0].vertexIndices.size(), 3);
  BOOST_CHECK_EQUAL(data->matchGroups[0].vertexIndices[0], 0);
  BOOST_CHECK_EQUAL(data->matchGroups[0].vertexIndices[1], 3);
  BOOST_CHECK_EQUAL(data->matchGroups[0].vertexIndices[2], 5);
  BOOST_CHECK_EQUAL(data->matchGroups[1].vertexIndices.size(), 2);
  BOOST_CHECK_EQUAL(data->matchGroups[1].vertexIndices[0], 2);
  BOOST_CHECK_EQUAL(data->matchGroups[1].vertexIndices[1], 6);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <Core/RaCore.hpp>

#include <Core/Containers/VectorArray.hpp>
#include <Core/Geometry/OpenMesh.hpp>
#include <Core/Geometry/TriangleMesh.hpp>
#include <Core/Types.hpp>
#include <Core/Utils/Index.hpp>
#include <Core/Utils/StdOptional.hpp>

#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Mesh/Traits.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Utils/Property.hh>
#include <OpenMesh/Core/Utils/PropertyManager.hh>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <set>

namespace Ra {
namespace Core {
namespace Geometry {
namespace deprecated {
using namespace Utils; // log, AttribXXX

/**
 * Define the Traits to be used by OpenMesh for TopologicalMesh.
 */
struct TopologicalMeshTraits : OpenMesh::DefaultTraits {
    using Point  = Ra::Core::Vector3;
    using Normal = Ra::Core::Vector3;

    VertexAttributes( OpenMesh::Attributes::Status | OpenMesh::Attributes::Normal );
    FaceAttributes( OpenMesh::Attributes::Status | OpenMesh::Attributes::Normal );
    EdgeAttributes( OpenMesh::Attributes::Status );
    // Add  OpenMesh::Attributes::PrevHalfedge for efficiency ?
    HalfedgeAttributes( OpenMesh::Attributes::Status | OpenMesh::Attributes::Normal );
};

/**
 * This class represents a mesh with topological information on the
 * vertex graph, using a half-edge representation.
 *
 * This integration is inspired by:
 * https://gist.github.com/Unril/03fa353d0461ed6bd41d
 *
 * \todo rename methods to respect Radium guideline (get/set/is, camelCase)
 * \todo private inheritance from OpenMesh, and import relevant methods.
 */
class RA_CORE_API TopologicalMesh : public OpenMesh::PolyMesh_ArrayKernelT<TopologicalMeshTraits>
{
  private:
    using base = OpenMesh::PolyMesh_ArrayKernelT<TopologicalMeshTraits>;
    using base::PolyMesh_ArrayKernelT;
    using Index   = Ra::Core::Utils::Index;
    using Vector3 = Ra::Core::Vector3;

  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    /**
     * Construct a topological mesh from a triangle mesh.
     * This operation merges vertex with same position, but keeps vertex
     * attributes on halfedges, so that TriangleMesh vertices with the same 3D
     * position are represented only once in the topological mesh.
     * \note This is a costly operation.
     *
     * \tparam NonManifoldFaceCommand Command executed when non-manifold faces are
     * found. API and default implementation:
     * \snippet Core/Geometry/TopologicalMesh.cpp Default command implementation
     *
     */
    template <typename NonManifoldFaceCommand>
    explicit TopologicalMesh( const Ra::Core::Geometry::TriangleMesh& triMesh,
                              NonManifoldFaceCommand command );

    /**
     * \brief Convenience constructor
     * \see TopologicalMesh( const Ra::Core::Geometry::TriangleMesh&, NonManifoldFaceCommand)
     */
    explicit TopologicalMesh( const Ra::Core::Geometry::TriangleMesh& triMesh );

    /**
     * Construct an empty topological mesh, only init mandatory properties.
     */
    explicit TopologicalMesh();

    /**
     * Return a triangleMesh from the topological mesh.
     * \note This is a costly operation.
     */
    TriangleMesh toTriangleMesh();

    /**
     * Update triangle mesh data, assuming the mesh and this topo mesh has the
     * same topology.
     * \warning note implemented yet.
     */
    void updateTriangleMesh( Ra::Core::Geometry::TriangleMesh& mesh );

    // import other version of halfedge_handle method
    using base::halfedge_handle;

    /**
     * Return the half-edge associated with a given vertex and face.
     * \note Asserts if vh is not a member of fh.
     */
    inline HalfedgeHandle halfedge_handle( VertexHandle vh, FaceHandle fh ) const;

    /**
     * Get normal of the vertex vh, when member of fh.
     * \note Asserts if vh is not a member of fh.
     */
    [[deprecated]] inline const Normal& normal( VertexHandle vh, FaceHandle fh ) const;

    /**
     * Set normal of the vertex vh, when member of fh.
     * \note Asserts if vh is not a member of fh.
     */
    [[deprecated]] void set_normal( VertexHandle vh, FaceHandle fh, const Normal& n );

    /// Import Base definition of normal and set normal.
    ///@{
    using base::normal;
    using base::set_normal;
    ///@}

    /**
     * Set the normal n to all the halfedges that points to vh (i.e. incomming
     * halfedges) .
     * If you work with vertex normals, please call this function on all vertex
     * handles before convertion with toTriangleMesh.
     */
    [[deprecated]] void propagate_normal_to_halfedges( VertexHandle vh );

    /**
     * Return a handle to the halfedge property storing vertices indices within
     * the TriangleMesh *this has been built on.
     */
    inline const OpenMesh::HPropHandleT<Index>& getInputTriangleMeshIndexPropHandle() const;

    /**
     * Return a handle to the halfedge property storing vertices indices within
     * the TriangleMesh returned by toTriangleMesh().
     * \note This property is valid only after toTriangleMesh() has been called.
     */
    inline const OpenMesh::HPropHandleT<Index>& getOutputTriangleMeshIndexPropHandle() const;

    /**
     * \name Const access to handles of the HalfEdge properties coming from
     * the TriangleMesh attributes.
     */
    ///@{
    [[deprecated]] inline const std::vector<OpenMesh::HPropHandleT<float>>&
    getFloatPropsHandles() const;
    [[deprecated]] inline const std::vector<OpenMesh::HPropHandleT<Vector2>>&
    getVector2PropsHandles() const;
    [[deprecated]] inline const std::vector<OpenMesh::HPropHandleT<Vector3>>&
    getVector3PropsHandles() const;
    [[deprecated]] inline const std::vector<OpenMesh::HPropHandleT<Vector4>>&
    getVector4PropsHandles() const;
    ///@}

    /**
     * \name Dealing with normals
     * Utils to deal with normals when modifying the mesh topology.
     */
    ///@{

    /**
     * Create a new property for normals on faces of \a mesh.
     * \note This new property will have to be propagated onto the newly created
     * halfedges with copyNormal().
     */
    inline void createNormalPropOnFaces( OpenMesh::FPropHandleT<Normal>& fProp );

    /**
     * Remove face property \a prop from \a mesh.
     * \note Invalidates the property handle.
     */
    inline void clearProp( OpenMesh::FPropHandleT<Normal>& fProp );

    /**
     * Copy the normal property from \a input_heh to \a copy_heh.
     */
    inline void copyNormal( HalfedgeHandle input_heh, HalfedgeHandle copy_heh );

    /** Copy the face normal property \a fProp from \a fh to \a heh.
     * \note \a fProp must have been previously created through createNormalPropOnFaces().
     */
    inline void
    copyNormalFromFace( FaceHandle fh, HalfedgeHandle heh, OpenMesh::FPropHandleT<Normal> fProp );

    /**
     * Interpolate normal property on edge center (after edge split).
     */
    inline void
    interpolateNormal( HalfedgeHandle in_a, HalfedgeHandle in_b, HalfedgeHandle out, Scalar f );

    /** Interpolate normal property on face center.
     * \note \a fProp must have been previously created through createNormalPropOnFaces().
     */
    inline void interpolateNormalOnFaces( FaceHandle fh, OpenMesh::FPropHandleT<Normal> fProp );
    ///@}

    /**
     * \name Dealing with custom properties
     * Utils to deal with custom properties of any type when modifying the mesh topology.
     */
    ///@{

    /**
     * Create a new property for each \a input properties of \a mesh on faces.
     * \note This new property will have to be propagated onto the newly created
     * halfedges with copyProps().
     */
    template <typename T>
    void createPropsOnFaces( const std::vector<OpenMesh::HPropHandleT<T>>& input,
                             std::vector<OpenMesh::FPropHandleT<T>>& output );

    /**
     * Remove \a props from \a mesh.
     * \note Clears \a props.
     */
    template <typename T>
    void clearProps( std::vector<OpenMesh::FPropHandleT<T>>& props );

    /**
     * Copy \a props properties from \a input_heh to \a copy_heh.
     */
    template <typename T>
    void copyProps( HalfedgeHandle input_heh,
                    HalfedgeHandle copy_heh,
                    const std::vector<OpenMesh::HPropHandleT<T>>& props );

    /**
     * Copy face properties \a props from \a fh to \a heh.
     * \note \a fProps must have been previously created through createPropsOnFaces().
     */
    template <typename T>
    void copyPropsFromFace( FaceHandle fh,
                            HalfedgeHandle heh,
                            const std::vector<OpenMesh::FPropHandleT<T>>& fProps,
                            const std::vector<OpenMesh::HPropHandleT<T>>& hProps );

    /**
     * Interpolate \a props on edge center (after edge split).
     */
    template <typename T>
    void interpolateProps( HalfedgeHandle in_a,
                           HalfedgeHandle in_b,
                           HalfedgeHandle out,
                           Scalar f,
                           const std::vector<OpenMesh::HPropHandleT<T>>& props );

    /**
     * Interpolate \a hprops on face center.
     * \note \a fProps must have been previously created through createPropsOnFaces().
     */
    template <typename T>
    void interpolatePropsOnFaces( FaceHandle fh,
                                  const std::vector<OpenMesh::HPropHandleT<T>>& hProps,
                                  const std::vector<OpenMesh::FPropHandleT<T>>& fProps );
    ///@}
    /**
        * \name Deal with all attributes* Utils to deal with the normal and
        custom properties when modifying the mesh topology.*/

    ///@{

    /**
     * Create a new property for each property of \a mesh on faces.
     * Outputs the new face properties handles in the corresponding output parameters.
     * \note These new properties will have to be propagated onto the newly created
     * halfedges with copyAllProps().
     */
    inline void createAllPropsOnFaces( OpenMesh::FPropHandleT<Normal>& normalProp,
                                       std::vector<OpenMesh::FPropHandleT<Scalar>>& floatProps,
                                       std::vector<OpenMesh::FPropHandleT<Vector2>>& vec2Props,
                                       std::vector<OpenMesh::FPropHandleT<Vector3>>& vec3Props,
                                       std::vector<OpenMesh::FPropHandleT<Vector4>>& vec4Props );

    /**
     * Remove all the given properties from \a mesh.
     * \note Invalidates \a normalProp and clears the given property containers.
     */
    inline void clearAllProps( OpenMesh::FPropHandleT<Normal>& normalProp,
                               std::vector<OpenMesh::FPropHandleT<Scalar>>& floatProps,
                               std::vector<OpenMesh::FPropHandleT<Vector2>>& vec2Props,
                               std::vector<OpenMesh::FPropHandleT<Vector3>>& vec3Props,
                               std::vector<OpenMesh::FPropHandleT<Vector4>>& vec4Props );

    /**
     * Copy all properties from \a input_heh to \a copy_heh.
     */
    inline void copyAllProps( HalfedgeHandle input_heh, HalfedgeHandle copy_heh );

    /**
     * Copy all given face properties from \a fh to \a heh.
     * \note Each property must have been previously created either all at once
     * through createAllPropsOnFaces(), or individually through
     * createNormalPropOnFaces() and createPropsOnFaces().
     */
    inline void copyAllPropsFromFace( FaceHandle fh,
                                      HalfedgeHandle heh,
                                      OpenMesh::FPropHandleT<Normal> normalProp,
                                      std::vector<OpenMesh::FPropHandleT<Scalar>>& floatProps,
                                      std::vector<OpenMesh::FPropHandleT<Vector2>>& vec2Props,
                                      std::vector<OpenMesh::FPropHandleT<Vector3>>& vec3Props,
                                      std::vector<OpenMesh::FPropHandleT<Vector4>>& vec4Props );

    /**
     * Interpolate all properties on edge center (after edge split).
     */
    inline void
    interpolateAllProps( HalfedgeHandle in_a, HalfedgeHandle in_b, HalfedgeHandle out, Scalar f );

    /**
     * Interpolate \a hprops on face center.
     * \note Each property must have been previously created either all at once
     * through createAllPropsOnFaces(), or individually through
     * createNormalPropOnFaces() and createPropsOnFaces().
     */
    inline void
    interpolateAllPropsOnFaces( FaceHandle fh,
                                OpenMesh::FPropHandleT<Normal> normalProp,
                                std::vector<OpenMesh::FPropHandleT<Scalar>>& floatProps,
                                std::vector<OpenMesh::FPropHandleT<Vector2>>& vec2Props,
                                std::vector<OpenMesh::FPropHandleT<Vector3>>& vec3Props,
                                std::vector<OpenMesh::FPropHandleT<Vector4>>& vec4Props );
    ///@}

    /**
     * \name Topological operations
     */
    ///@{
    /**
     * Apply a 2-4 edge split.
     * \param eh The handle to the edge to split.
     * \param f The interpolation factor to place the new point on the edge.
     *          Must be in [0,1].
     * \return True if the edge has been split, false otherwise.
     * \note Only applies on edges between 2 triangles, and if \a f is in [0,1].
     * \note Mesh attributes are linearly interpolated on the newly created
     * halfedge.
     * \note f=0 correspond to halfedge_handle( eh, 0 ) (i.e. first vertex of
     * the edge)
     */
    bool splitEdge( TopologicalMesh::EdgeHandle eh, Scalar f );
    bool splitEdgeWedge( TopologicalMesh::EdgeHandle eh, Scalar f );

    ///@}

  private:
    template <typename T>
    using HandleAndValueVector =
        std::vector<std::pair<AttribHandle<T>, T>,
                    Eigen::aligned_allocator<std::pair<AttribHandle<T>, T>>>;

    template <typename T>
    using PropPair = std::pair<AttribHandle<T>, OpenMesh::HPropHandleT<T>>;

    template <typename T>
    inline void copyAttribToTopo( const TriangleMesh& triMesh,
                                  const std::vector<PropPair<T>>& vprop,
                                  TopologicalMesh::HalfedgeHandle heh,
                                  unsigned int vindex );

    template <typename T>
    inline void addAttribPairToTopo( const TriangleMesh& triMesh,
                                     AttribManager::pointer_type attr,
                                     std::vector<PropPair<T>>& vprop,
                                     std::vector<OpenMesh::HPropHandleT<T>>& pph );

    void split_copy( EdgeHandle _eh, VertexHandle _vh );
    void split( EdgeHandle _eh, VertexHandle _vh );

    ///\todo to be deleted/updated
    OpenMesh::HPropHandleT<Index> m_inputTriangleMeshIndexPph;
    OpenMesh::HPropHandleT<Index> m_outputTriangleMeshIndexPph;
    [[deprecated]] std::vector<OpenMesh::HPropHandleT<float>> m_floatPph;
    [[deprecated]] std::vector<OpenMesh::HPropHandleT<Vector2>> m_vec2Pph;
    [[deprecated]] std::vector<OpenMesh::HPropHandleT<Vector3>> m_vec3Pph;
    [[deprecated]] std::vector<OpenMesh::HPropHandleT<Vector4>> m_vec4Pph;

    friend class TMOperations;
};

} // namespace deprecated
} // namespace Geometry
} // namespace Core
} // namespace Ra
#include <Core/Geometry/deprecated/TopologicalMesh.inl>


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

#ifndef SAMPLING_HPP
#define SAMPLING_HPP

#include <math.h>

#include <Eigen/Core>

#include <ippp/Identifier.h>
#include <ippp/modules/collisionDetection/CollisionDetection.hpp>
#include <ippp/modules/sampler/Sampler.hpp>
#include <ippp/modules/trajectoryPlanner/TrajectoryPlanner.hpp>
#include <ippp/environment/Environment.h>

namespace ippp {

/*!
* \brief   Class Sampling creates sample Vectors with the passed strategy, for creating single Vectors the Sampler is used.
* \author  Sascha Kaden
* \date    2016-12-20
*/
template <unsigned int dim>
class Sampling : public Identifier {
  public:
    Sampling(const std::string &name, const std::shared_ptr<Environment> &environment, const std::shared_ptr<CollisionDetection<dim>> &collision,
             const std::shared_ptr<TrajectoryPlanner<dim>> &trajectory, const std::shared_ptr<Sampler<dim>> &sampler,
             const size_t attempts = 10);

    virtual Vector<dim> getSample() = 0;
    virtual Vector<dim> getSample(const Vector<dim> &prevSample);
    virtual std::vector<Vector<dim>> getSamples(const size_t amount);

    std::shared_ptr<Sampler<dim>> getSampler() const;
    double getRandomNumber() const;
    void setOrigin(const Vector<dim> &origin);

    void setRobotBoundings(const std::pair<Vector<dim>, Vector<dim>> &robotBoundings);
    bool checkRobotBounding(const Vector<dim> &config) const;

  protected:
    const size_t m_attempts;
    std::pair<Vector<dim>, Vector<dim>> m_robotBounding;

    std::shared_ptr<CollisionDetection<dim>> m_collision = nullptr;
    std::shared_ptr<Environment> m_environment = nullptr;
    std::shared_ptr<Sampler<dim>> m_sampler = nullptr;
    std::shared_ptr<TrajectoryPlanner<dim>> m_trajectory = nullptr;
};

/*!
*  \brief      Constructor of the class Sampling
*  \author     Sascha Kaden
*  \param[in]  name
*  \param[in]  Environment
*  \param[in]  CollisionDetection
*  \param[in]  TrajectoryPlanner
*  \param[in]  Sampler
*  \param[in]  attempts for one sampling
*  \date       2016-12-20
*/
template <unsigned int dim>
Sampling<dim>::Sampling(const std::string &name, const std::shared_ptr<Environment> &environment,
                        const std::shared_ptr<CollisionDetection<dim>> &collision,
                        const std::shared_ptr<TrajectoryPlanner<dim>> &trajectory, const std::shared_ptr<Sampler<dim>> &sampler,
                        const size_t attempts)
    : Identifier(name), m_environment(environment), m_collision(collision), m_trajectory(trajectory), m_sampler(sampler), m_attempts(attempts) {
    setRobotBoundings(m_environment->getRobotBoundaries());
    Logging::debug("Initialize", this);
}

/*!
*  \brief      Return sample
*  \author     Sascha Kaden
*  \param[in]  previous sample
*  \param[out] sample
*  \date       2017-06-07
*/
template <unsigned int dim>
Vector<dim> Sampling<dim>::getSample(const Vector<dim> &prevSample) {
    return getSample();
}

/*!
*  \brief      Return list of samples
*  \author     Sascha Kaden
*  \param[in]  number of samples
*  \param[out] list of samples
*  \date       2017-06-07
*/
template <unsigned int dim>
std::vector<Vector<dim>> Sampling<dim>::getSamples(const size_t amount) {
    std::vector<Vector<dim>> samples;
    samples.reserve(amount);
    for (size_t i = 0; i < amount; ++i)
        samples.push_back(getSample());

    return samples;
}

/*!
*  \brief      Return the Sampler of the Sampling strategy.
*  \author     Sascha Kaden
*  \param[out] Sampler instance
*  \date       2017-11-14
*/
template <unsigned int dim>
std::shared_ptr<Sampler<dim>> Sampling<dim>::getSampler() const {
    return m_sampler;
}

/*!
*  \brief      Return random number between 0 and 1
*  \author     Sascha Kaden
*  \param[out] random number
*  \date       2017-04-03
*/
template <unsigned int dim>
double Sampling<dim>::getRandomNumber() const {
    return m_sampler->getRandomNumber();
}

/*!
*  \brief      Set the origin of the Sampler
*  \author     Sascha Kaden
*  \param[in]  origin
*  \date       2016-12-20
*/
template <unsigned int dim>
void Sampling<dim>::setOrigin(const Vector<dim> &origin) {
    m_sampler->setOrigin(origin);
}

/*!
*  \brief      Sets the robot boundings of all robots, dimension should be the same.
*  \author     Sascha Kaden
*  \param[in]  pair of min and max boundary Vector
*  \date       2017-11-14
*/
template <unsigned int dim>
void Sampling<dim>::setRobotBoundings(const std::pair<Vector<dim>, Vector<dim>> &robotBoundings) {
    m_robotBounding = robotBoundings;
}

/*!
*  \brief      Checks the boundaries of the robot to the passed configuration, return true if valid.
*  \author     Sascha Kaden
*  \param[in]  configuration
*  \param[out] validity of boundary check.
*  \date       2017-11-14
*/
template <unsigned int dim>
bool Sampling<dim>::checkRobotBounding(const Vector<dim> &config) const {
    for (unsigned int i = 0; i < dim; ++i) {
        if (config[i] <= m_robotBounding.first[i] || m_robotBounding.second[i] <= config[i]) {
            Logging::trace("Robot out of robot boundary", this);
            return true;
        }
    }
    return false;
}

} /* namespace ippp */

#endif /* SAMPLING_HPP */