/*=============================================================================
    Copyright (c) 2001-2007 Joel de Guzman

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

#include <boost/spirit/home/phoenix/core/composite.hpp>
#include <boost/spirit/home/phoenix/core/compose.hpp>
#include <boost/spirit/home/phoenix/detail/type_deduction.hpp>
#include <boost/spirit/home/phoenix/operator/detail/unary_eval.hpp>
#include <boost/spirit/home/phoenix/operator/detail/unary_compose.hpp>
#include <boost/spirit/home/phoenix/operator/detail/binary_eval.hpp>
#include <boost/spirit/home/phoenix/operator/detail/binary_compose.hpp>

namespace boost { namespace phoenix
{
    struct negate_eval;
    struct posit_eval;
    struct pre_increment_eval;
    struct pre_decrement_eval;
    struct post_increment_eval;
    struct post_decrement_eval;

    struct plus_assign_eval;
    struct minus_assign_eval;
    struct multiplies_assign_eval;
    struct divides_assign_eval;
    struct modulus_assign_eval;

    struct plus_eval;
    struct minus_eval;
    struct multiplies_eval;
    struct divides_eval;
    struct modulus_eval;

    BOOST_UNARY_RESULT_OF(-x, result_of_negate)
    BOOST_UNARY_RESULT_OF(+x, result_of_posit)
    BOOST_UNARY_RESULT_OF(++x, result_of_pre_increment)
    BOOST_UNARY_RESULT_OF(--x, result_of_pre_decrement)
    BOOST_UNARY_RESULT_OF(x++, result_of_post_increment)
    BOOST_UNARY_RESULT_OF(x--, result_of_post_decrement)

    BOOST_BINARY_RESULT_OF(x += y, result_of_plus_assign)
    BOOST_BINARY_RESULT_OF(x -= y, result_of_minus_assign)
    BOOST_BINARY_RESULT_OF(x *= y, result_of_multiplies_assign)
    BOOST_BINARY_RESULT_OF(x /= y, result_of_divides_assign)
    BOOST_BINARY_RESULT_OF(x %= y, result_of_modulus_assign)

    BOOST_BINARY_RESULT_OF(x + y, result_of_plus)
    BOOST_BINARY_RESULT_OF(x - y, result_of_minus)
    BOOST_BINARY_RESULT_OF(x * y, result_of_multiplies)
    BOOST_BINARY_RESULT_OF(x / y, result_of_divides)
    BOOST_BINARY_RESULT_OF(x % y, result_of_modulus)

#define x a0.eval(env)
#define y a1.eval(env)

    PHOENIX_UNARY_EVAL(negate_eval, result_of_negate, -x)
    PHOENIX_UNARY_EVAL(posit_eval, result_of_posit, +x)
    PHOENIX_UNARY_EVAL(pre_increment_eval, result_of_pre_increment, ++x)
    PHOENIX_UNARY_EVAL(pre_decrement_eval, result_of_pre_decrement, --x)
    PHOENIX_UNARY_EVAL(post_increment_eval, result_of_post_increment, x++)
    PHOENIX_UNARY_EVAL(post_decrement_eval, result_of_post_decrement, x--)

    PHOENIX_BINARY_EVAL(plus_assign_eval, result_of_plus_assign, x += y)
    PHOENIX_BINARY_EVAL(minus_assign_eval, result_of_minus_assign, x -= y)
    PHOENIX_BINARY_EVAL(multiplies_assign_eval, result_of_multiplies_assign, x *= y)
    PHOENIX_BINARY_EVAL(divides_assign_eval, result_of_divides_assign, x /= y)
    PHOENIX_BINARY_EVAL(modulus_assign_eval, result_of_modulus_assign, x %= y)

    PHOENIX_BINARY_EVAL(plus_eval, result_of_plus, x + y)
    PHOENIX_BINARY_EVAL(minus_eval, result_of_minus, x - y)
    PHOENIX_BINARY_EVAL(multiplies_eval, result_of_multiplies, x * y)
    PHOENIX_BINARY_EVAL(divides_eval, result_of_divides, x / y)
    PHOENIX_BINARY_EVAL(modulus_eval, result_of_modulus, x % y)

    PHOENIX_UNARY_COMPOSE(negate_eval, -)
    PHOENIX_UNARY_COMPOSE(posit_eval, +)
    PHOENIX_UNARY_COMPOSE(pre_increment_eval, ++)
    PHOENIX_UNARY_COMPOSE(pre_decrement_eval, --)

    template <typename T0>
    inline actor<typename as_composite<post_increment_eval, actor<T0> >::type>
    operator++(actor<T0> const& a0, int) // special case
    {
        return compose<post_increment_eval>(a0);
    }

    template <typename T0>
    inline actor<typename as_composite<post_decrement_eval, actor<T0> >::type>
    operator--(actor<T0> const& a0, int) // special case
    {
        return compose<post_decrement_eval>(a0);
    }

    PHOENIX_BINARY_COMPOSE(plus_assign_eval, +=)
    PHOENIX_BINARY_COMPOSE(minus_assign_eval, -=)
    PHOENIX_BINARY_COMPOSE(multiplies_assign_eval, *=)
    PHOENIX_BINARY_COMPOSE(divides_assign_eval, /=)
    PHOENIX_BINARY_COMPOSE(modulus_assign_eval, %=)

    PHOENIX_BINARY_COMPOSE(plus_eval, +)
    PHOENIX_BINARY_COMPOSE(minus_eval, -)
    PHOENIX_BINARY_COMPOSE(multiplies_eval, *)
    PHOENIX_BINARY_COMPOSE(divides_eval, /)
    PHOENIX_BINARY_COMPOSE(modulus_eval, %)

#undef x
#undef y
}}

#endif


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

#include <boost/simd/arithmetic/functions/muls.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/shift_right.hpp>
#include <boost/simd/include/functions/simd/is_not_equal.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/split_multiplies.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/genmask.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>

/* No native groups for 64-bit SSE;
 * we use bit tricks instead of calling saturate. */

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( muls_, boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<uint32_<A0>, boost::simd::tag::sse_>))
                                      ((simd_<uint32_<A0>, boost::simd::tag::sse_>))
                                    )
  {

    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      typedef typename dispatch::meta::upgrade<A0>::type utype;

      utype res0, res1;
      split_multiplies(a0, a1, res0, res1);

      return group(res0, res1)
           | genmask( group( shrai(res0, sizeof(stype)*CHAR_BIT)
                           , shrai(res1, sizeof(stype)*CHAR_BIT)
                           )
                    );
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( muls_, boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_<int32_<A0>, boost::simd::tag::sse_>))
                                      ((simd_<int32_<A0>, boost::simd::tag::sse_>))
                                    )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::scalar_of<A0>::type stype;
      typedef typename dispatch::meta::as_unsigned<A0>::type untype;
      typedef typename dispatch::meta::upgrade<A0>::type utype;

      utype res0, res1;
      split_multiplies(a0, a1, res0, res1);

      untype res2 = shrai(bitwise_cast<untype>(a0 ^ a1), sizeof(stype)*CHAR_BIT-1)
                  + static_cast<typename meta::scalar_of<untype>::type>(Valmax<stype>());

      A0 hi = group( shrai(res0, sizeof(stype)*CHAR_BIT)
                   , shrai(res1, sizeof(stype)*CHAR_BIT)
                   );
      A0 lo = group(res0, res1);

      return if_else( hi != shrai(lo, sizeof(stype)*CHAR_BIT-1)
                    , bitwise_cast<A0>(res2)
                    , lo
                    );
    }
  };
} } }

#endif
#endif


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <Eigen/Dense>
#include <pinocchio/multibody/model.hpp>
#include "tsid/tasks/task-contact-force-equality.hpp"

namespace tsid {
namespace tasks {

using namespace tsid::math;
using namespace std;

TaskContactForceEquality::TaskContactForceEquality(const std::string & name, RobotWrapper & robot,
                                                   const double dt, contacts::ContactBase & contact):
  TaskContactForce(name, robot),
  m_contact(&contact),
  m_constraint(name, 6, 12),
  m_ref(6,6),
  m_fext(6,6) {
  m_forceIntegralError = Vector::Zero(6);
  m_dt = dt;
  m_leak_rate = 0.05;
  m_contact_name = m_contact->name();
}

int TaskContactForceEquality::dim() const {
  return 6;
}

const Vector & TaskContactForceEquality::Kp() const { return m_Kp; }
const Vector & TaskContactForceEquality::Kd() const { return m_Kd; }
const Vector & TaskContactForceEquality::Ki() const { return m_Ki; }
const double & TaskContactForceEquality::getLeakRate() const { return m_leak_rate; }

void TaskContactForceEquality::Kp(ConstRefVector Kp)
{
  assert(Kp.size()==6);
  m_Kp = Kp;
}

void TaskContactForceEquality::Kd(ConstRefVector Kd)
{
  assert(Kd.size()==6);
  m_Kd = Kd;
}

void TaskContactForceEquality::Ki(ConstRefVector Ki)
{
  assert(Ki.size()==6);
  m_Ki = Ki;
}

void TaskContactForceEquality::setLeakRate(double leak)
{
  m_leak_rate = leak;
}

const std::string& TaskContactForceEquality::getAssociatedContactName() {
  return m_contact_name;
}

const contacts::ContactBase& TaskContactForceEquality::getAssociatedContact() {
  return *m_contact;
}

void TaskContactForceEquality::setAssociatedContact(contacts::ContactBase & contact) {
  m_contact = &contact;
  m_contact_name = m_contact->name();
}

void TaskContactForceEquality::setReference(TrajectorySample & ref) {
  m_ref = ref;
}

const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getReference() const {
  return m_ref;
}

void TaskContactForceEquality::setExternalForce(TrajectorySample & f_ext) {
  m_fext = f_ext;
}

const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getExternalForce() const {
  return m_fext;
}

const ConstraintBase & TaskContactForceEquality::compute(const double t,
                                                         ConstRefVector q,
                                                         ConstRefVector v,
                                                         Data & data,
                                                         const std::vector<std::shared_ptr<ContactLevel> >  *contacts) {
  bool contactFound = false;
  if (m_contact_name != "") {
    // look if the associated contact is in the list of contact
    for(auto cl : *contacts) {
      if (m_contact_name == cl->contact.name()) {
        contactFound = true;
        break;
      }
    }
  } else {
    std::cout << "[TaskContactForceEquality] ERROR: Contact name empty" << std::endl;
    return m_constraint;
  }
  if (!contactFound) {
    std::cout << "[TaskContactForceEquality] ERROR: Contact name not in the list of contact in the formulation pb" << std::endl;
    return m_constraint;
  }
  return compute(t, q, v, data);
}

const ConstraintBase & TaskContactForceEquality::compute(const double,
                                                         ConstRefVector,
                                                         ConstRefVector,
                                                         Data & data) {

  auto& M = m_constraint.matrix();
  M = m_contact->getForceGeneratorMatrix(); // 6x12 for a 6d contact

  Vector forceError = m_ref.getValue() - m_fext.getValue();
  Vector f_ref = m_ref.getValue() + m_Kp.cwiseProduct(forceError) + m_Kd.cwiseProduct(m_ref.getDerivative() - m_fext.getDerivative())
                 + m_Ki.cwiseProduct(m_forceIntegralError);
  m_constraint.vector() = f_ref;

  m_forceIntegralError += (forceError - m_leak_rate * m_forceIntegralError) * m_dt;

  return m_constraint;
}

const ConstraintBase & TaskContactForceEquality::getConstraint() const {
  return m_constraint;
}

}
}


//
// Created by Hamza El-Kebir on 2/20/22.
//

#ifndef LSEXAMPLE_PANDAARMBLOCK_HPP
#define LSEXAMPLE_PANDAARMBLOCK_HPP

#include <Lodestar/blocks/Block.hpp>
#include <Eigen/Dense>
#include <string>
#include <array>

#include "arm/Panda.hpp"
#include <thread>

namespace ls {
    namespace blocks {
        class PandaArmBlock
                : public Block<
                        ::std::tuple<Eigen::Matrix<double, 7, 1>>,
                        ::std::tuple<PandaState>,
                        ::std::tuple<::std::string>
                > {
        public:
            using Base =
            Block<
                    ::std::tuple<Eigen::Matrix<double, 7, 1>>,
                    ::std::tuple<PandaState>,
                    ::std::tuple<::std::string>
            >;

            using Matrix = Eigen::Matrix<double, 7, 1>;

            PandaArmBlock()
            {
                this->template i<0>().object.setZero();
                bindFunction();
            }

            ~PandaArmBlock()
            {
                panda_.stop();
            }

            typename ::std::tuple_element<0, typename Base::Params>::type &
            ipAddress()
            {
                return this->template p<0>();
            }

            const typename ::std::tuple_element<0, typename Base::Params>::type &
            ipAddress() const
            {
                return this->template p<0>();
            }

        protected:
            Panda panda_;

            void bindFunction()
            {
                this->equation = ::std::bind(
                        &PandaArmBlock::triggerFunction, this,
                        ::std::placeholders::_1);
            }

            void triggerFunction(Base &b)
            {
                static bool init = false;
                static bool ready = false;

                if (!init) {
                    panda_ = Panda{ipAddress(), false};

                    init = panda_.initConnection().ok();

                    if (!init)
                        return;

                    Eigen::AngleAxisd aa{M_PI / 2, Eigen::Vector3d{0, -1, 0}};
                    Eigen::AngleAxisd aa2{M_PI, Eigen::Vector3d{1, 0, 0}};
                    aa = aa * aa2;
                    Eigen::Vector3d translation = {200e-3, 0, 0};

                    panda_.setNominalToEndEffectorTransformation(aa, translation);

                    panda_.setGuidingMode();

                    ready = true;

                    ::std::thread([&] () {
                        panda_.robot_->control([&] (const franka::RobotState &rs, franka::Duration duration) -> franka::Torques
                                               {
                                                   // TODO: Add mutex for thread safety

                                                   this->template o<0>().object.robot.parse(rs);
                                                   this->template o<0>().propagate();

                                                   while (!ready) {}

                                                   ::std::array<double, 7> tau_d_array{this->template i<0>().object[0],
                                                                                this->template i<0>().object[1],
                                                                                this->template i<0>().object[2],
                                                                                this->template i<0>().object[3],
                                                                                this->template i<0>().object[4],
                                                                                this->template i<0>().object[5],
                                                                                this->template i<0>().object[6]};
                                                   ready = false;
                                                   return {tau_d_array};
                                               });
//                        panda_.control(f);
                    }).detach();
                }

                ready = true;

//                b.template o<0>() = panda_.getState().value();
            }
        };

        template<>
        class BlockTraits<PandaArmBlock> {
        public:
            static constexpr const BlockType blockType = BlockType::CustomBlock;
            enum {
                directFeedthrough = false
            };

            using type = PandaArmBlock;
            using Base = typename type::Base;

            enum {
                kIns = Base::kIns,
                kOuts = Base::kOuts,
                kPars = Base::kPars
            };

            static const ::std::array<::std::string, kIns> inTypes;
            static const ::std::array<::std::string, kOuts> outTypes;
            static const ::std::array<::std::string, kPars> parTypes;

            static const ::std::array<::std::string, 0> templateTypes;
        };
    }
}

#endif //LSEXAMPLE_PANDAARMBLOCK_HPP


// Copyright 2019-present MongoDB Inc.
//
// 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 HEADER_EBA231D0_AA7A_4008_A9E8_BD1C98D9023E_INCLUDED
#define HEADER_EBA231D0_AA7A_4008_A9E8_BD1C98D9023E_INCLUDED

#include <cstdint>
#include <memory>

#include <boost/random.hpp>

namespace genny {
namespace v1 {

/**
 * Genny random number generator.
 *
 * @tparam RNGImpl Random number generator implementation to use.
 * @private
 */
template <class RNGImpl>
class Random {

public:
    using result_type = typename RNGImpl::result_type;

    /** @private */
    using Handle = Random&;

    /**
     * Construct a Random object.
     * @param seed the seed. The default seed is used if seed is omitted.
     */
    explicit Random(result_type seed = 6514393) : _rng(seed) {}

    // Moves are okay
    Random(Random&&) noexcept = default;
    Random& operator=(Random&&) noexcept = default;

    // But no copies
    Random(const Random&) = delete;
    Random& operator=(const Random&) = delete;

    ~Random() = default;

    /**
     * Construct new Random using the next number from the current one as the seed.
     * @return
     */
    Random child() {
        return Random(this->nextValue());
    }

    /**
     * Seed engine.
     */
    void seed(result_type newSeed) {
        _rng.seed(newSeed);
    }

    /**
     * Generate random number.
     */
    result_type nextValue() {
        return _rng();
    }

    /**
     * Generate random number, shorthand for Random::nextValue()
     */
    result_type operator()() {
        return this->nextValue();
    }

    /**
     * Minimum value.
     */
    static constexpr auto min() {
        return RNGImpl::min();
    }

    /**
     * Maximum value.
     */
    static constexpr auto max() {
        return RNGImpl::max();
    }

private:
    // RNGImpl is a plain class member instead of a unique pointer to avoid performance penalty.
    // For more detail, see https://github.com/10gen/genny/pull/88#issuecomment-451014165
    RNGImpl _rng;
};
}  // namespace v1

/**
 * DefaultRandom should be used if you need a random number generator.
 */
// Note we use boost::random because its distributions are
// cross-platform.
using DefaultRandom = v1::Random<boost::random::mt19937_64>;

}  // namespace genny
#endif  // HEADER_EBA231D0_AA7A_4008_A9E8_BD1C98D9023E_INCLUDED


// Copyright 2021 Tier IV, Inc.
//
// 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 UTILIZATION__ARC_LANE_UTIL_HPP_
#define UTILIZATION__ARC_LANE_UTIL_HPP_

#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/geometry/geometry.hpp>
#include <utilization/boost_geometry_helper.hpp>

#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>

#include <boost/optional.hpp>

#ifdef ROS_DISTRO_GALACTIC
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#else
#include <tf2_eigen/tf2_eigen.hpp>

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#endif

#include <algorithm>
#include <memory>
#include <utility>
#include <vector>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>

namespace behavior_velocity_planner
{
namespace bg = boost::geometry;
namespace arc_lane_utils
{
using PathIndexWithPose = std::pair<size_t, geometry_msgs::msg::Pose>;  // front index, pose
using PathIndexWithPoint2d = std::pair<size_t, Point2d>;                // front index, point2d
using PathIndexWithOffset = std::pair<size_t, double>;                  // front index, offset

inline double calcSignedDistance(
  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2)
{
  Eigen::Affine3d map2p1;
  tf2::fromMsg(p1, map2p1);
  const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z);
  return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm();
}

inline boost::optional<Point2d> getNearestCollisionPoint(
  const LineString2d & stop_line, const LineString2d & path_segment)
{
  // Find all collision points
  std::vector<Point2d> collision_points;
  bg::intersection(stop_line, path_segment, collision_points);
  if (collision_points.empty()) {
    return {};
  }

  // To dist list
  std::vector<double> dist_list;
  dist_list.reserve(collision_points.size());
  std::transform(
    collision_points.cbegin(), collision_points.cend(), std::back_inserter(dist_list),
    [&path_segment](const Point2d & collision_point) {
      return bg::distance(path_segment.front(), collision_point);
    });

  // Find nearest collision point
  const auto min_itr = std::min_element(dist_list.cbegin(), dist_list.cend());
  const auto min_idx = std::distance(dist_list.cbegin(), min_itr);

  return collision_points.at(min_idx);
}

template <class T>
boost::optional<PathIndexWithPoint2d> findCollisionSegment(
  const T & path, const LineString2d & stop_line)
{
  for (size_t i = 0; i < path.points.size() - 1; ++i) {
    const auto & p1 = path.points.at(i).point.pose.position;      // Point before collision point
    const auto & p2 = path.points.at(i + 1).point.pose.position;  // Point after collision point

    const LineString2d path_segment = {{p1.x, p1.y}, {p2.x, p2.y}};

    const auto nearest_collision_point = getNearestCollisionPoint(stop_line, path_segment);
    if (nearest_collision_point) {
      return std::make_pair(i, *nearest_collision_point);
    }
  }

  return {};
}

template <class T>
boost::optional<PathIndexWithOffset> findForwardOffsetSegment(
  const T & path, const size_t base_idx, const double offset_length)
{
  double sum_length = 0.0;
  for (size_t i = base_idx; i < path.points.size() - 1; ++i) {
    sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));

    // If it's over offset point, return front index and remain offset length
    if (sum_length >= offset_length) {
      return std::make_pair(i, sum_length - offset_length);
    }
  }

  // No enough path length
  return {};
}

template <class T>
boost::optional<PathIndexWithOffset> findBackwardOffsetSegment(
  const T & path, const size_t base_idx, const double offset_length)
{
  double sum_length = 0.0;
  const auto start = static_cast<std::int32_t>(base_idx) - 1;
  for (std::int32_t i = start; i >= 0; --i) {
    sum_length += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));

    // If it's over offset point, return front index and remain offset length
    if (sum_length >= offset_length) {
      const auto k = static_cast<std::size_t>(i);
      return std::make_pair(k, sum_length - offset_length);
    }
  }

  // No enough path length
  return {};
}

template <class T>
boost::optional<PathIndexWithOffset> findOffsetSegment(
  const T & path, const PathIndexWithPoint2d & collision_segment, const double offset_length)
{
  const size_t collision_idx = collision_segment.first;
  const Point2d & collision_point = collision_segment.second;
  const auto p_front = to_bg2d(path.points.at(collision_idx).point.pose.position);
  const auto p_back = to_bg2d(path.points.at(collision_idx + 1).point.pose.position);

  if (offset_length >= 0) {
    return findForwardOffsetSegment(
      path, collision_idx, offset_length + bg::distance(p_front, collision_point));
  } else {
    return findBackwardOffsetSegment(
      path, collision_idx + 1, -offset_length + bg::distance(p_back, collision_point));
  }
}

template <class T>
geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment)
{
  const size_t offset_idx = offset_segment.first;
  const double remain_offset_length = offset_segment.second;
  const auto & p_front = path.points.at(offset_idx).point.pose.position;
  const auto & p_back = path.points.at(offset_idx + 1).point.pose.position;

  // To Eigen point
  const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y);
  const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y);

  // Calculate interpolation ratio
  const auto interpolate_ratio = remain_offset_length / (p_eigen_back - p_eigen_front).norm();

  // Add offset to front point
  const auto target_point_2d = p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front);
  const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z);

  // Calculate orientation so that X-axis would be along the trajectory
  geometry_msgs::msg::Pose target_pose;
  target_pose.position.x = target_point_2d.x();
  target_pose.position.y = target_point_2d.y();
  target_pose.position.z = interpolated_z;
  const double yaw = tier4_autoware_utils::calcAzimuthAngle(p_front, p_back);
  target_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
  return target_pose;
}

}  // namespace arc_lane_utils
}  // namespace behavior_velocity_planner

#endif  // UTILIZATION__ARC_LANE_UTIL_HPP_


//---------------------------------------------------------------------------//
//  MIT License
//
//  Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in all
//  copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//  SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE filecoin_pieces_test

#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/unit_test.hpp>

#include <nil/filecoin/proofs/pieces.hpp>

BOOST_AUTO_TEST_SUITE(filecoin_pieces_test_suite)

template<typename PieceSizesIterator>
std::tuple<std::array<std::uint8_t, 32>, piece_info>
    build_sector(PieceSizesIterator piece_sizes_first, PieceSizesIterator pieces_sizes_last, sector_size ss) {
    let rng = &mut XorShiftRng::from_seed(crate::TEST_SEED);
    let porep_id = [32; 32];
    let graph = StackedBucketGraph<DefaultPieceHasher>::new_stacked(
        sector_size / NODE_SIZE, DRG_DEGREE, EXP_DEGREE, porep_id);

    let mut staged_sector = Vec::with_capacity(u64::from(sector_size) as usize);
    let mut staged_sector_io = std::io::Cursor::new (&mut staged_sector);
    let mut piece_infos = Vec::with_capacity(piece_sizes.len());

    for (i, piece_size)
        in piece_sizes.iter().enumerate() {
            let piece_size_u = u64::from(*piece_size) as usize;
            let mut piece_bytes = vec ![255u8; piece_size_u];
            rng.fill_bytes(&mut piece_bytes);

            let mut piece_file = std::io::Cursor::new (&mut piece_bytes);

            let(piece_info, _) =
                crate::api::add_piece(&mut piece_file, &mut staged_sector_io, *piece_size, &piece_sizes[..i], ) ?
                ;

            piece_infos.push(piece_info);
        }
    BOOST_CHECK_EQUAL(staged_sector.len(), u64::from(sector_size) as usize);

    let data_tree : DataTree = create_base_merkle_tree::<DataTree>(None, graph.size(), &staged_sector);
    let comm_d_root : Fr = data_tree.root().into();
    let comm_d = commitment_from_fr(comm_d_root);

    return std::make_tuple(comm_d, piece_infos);
}

std::uint32_t prev_power_of_two(std::uint32_t x) {
    x |= x >> 1;
    x |= x >> 2;
    x |= x >> 4;
    x |= x >> 8;
    x |= x >> 16;
    return x - (x >> 1);
}

BOOST_AUTO_TEST_CASE(test_empty_source) {
    let mut source = EmptySource::new (12);
    let mut target = Vec::new ();
    source.read_to_end(&mut target);
    BOOST_CHECK_EQUAL(target, vec ![0u8; 12]);
}

BOOST_AUTO_TEST_CASE(test_compute_comm_d_empty) {
    let comm_d = compute_comm_d(SectorSize(2048), &[]);
    BOOST_CHECK_EQUAL(comm_d, [
        252, 126, 146, 130, 150, 229, 22, 250, 173, 233, 134, 178, 143, 146, 212, 74,
        79,  36,  185, 53,  72,  82,  35, 55,  106, 121, 144, 39,  188, 24,  248, 51
    ]);

    let comm_d = compute_comm_d(SectorSize(128), &[]);
    BOOST_CHECK_EQUAL(hex::encode(&comm_d), "3731bb99ac689f66eef5973e4a94da188f4ddcae580724fc6f3fd60dfd488333", );
}

BOOST_AUTO_TEST_CASE(test_get_piece_alignment) {
    let table = vec ![
        (0, 0, (0, 127)),
        (0, 127, (0, 0)),
        (0, 254, (0, 0)),
        (0, 508, (0, 0)),
        (0, 1016, (0, 0)),
        (127, 127, (0, 0)),
        (127, 254, (127, 0)),
        (127, 508, (381, 0)),
        (100, 100, (27, 27)),
        (200, 200, (54, 54)),
        (300, 300, (208, 208)),
    ];

    for (bytes_in_sector, bytes_in_piece, (expected_left_align, expected_right_align))
        in table.clone() {
            let PieceAlignment {
                left_bytes : UnpaddedBytesAmount(actual_left_align),
                right_bytes : UnpaddedBytesAmount(actual_right_align),
            } = get_piece_alignment(UnpaddedBytesAmount(bytes_in_sector), UnpaddedBytesAmount(bytes_in_piece), );
            BOOST_CHECK_EQUAL((expected_left_align, expected_right_align), (actual_left_align, actual_right_align));
        }
}

BOOST_AUTO_TEST_CASE(test_get_piece_start_byte) {
    let pieces = [
        UnpaddedBytesAmount(31),
        UnpaddedBytesAmount(32),
        UnpaddedBytesAmount(33),
    ];

    BOOST_CHECK_EQUAL(get_piece_start_byte(&pieces[..0], pieces[0]), UnpaddedByteIndex(0));
    BOOST_CHECK_EQUAL(get_piece_start_byte(&pieces[..1], pieces[1]), UnpaddedByteIndex(127));
    BOOST_CHECK_EQUAL(get_piece_start_byte(&pieces[..2], pieces[2]), UnpaddedByteIndex(254));
}

BOOST_AUTO_TEST_CASE(test_verify_simple_pieces) {
    let rng = &mut XorShiftRng::from_seed(crate::TEST_SEED);

    //     g
    //   /  \
        //  e    f
    // / \  / \
        // a  b c  d

    let(a, b, c, d) : ([u8; 32], [u8; 32], [u8; 32], [u8; 32]) = rng.gen();

    let mut e = [0u8; 32];
    let h = piece_hash(&a, &b);
    e.copy_from_slice(h.as_ref());

    let mut f = [0u8; 32];
    let h = piece_hash(&c, &d);
    f.copy_from_slice(h.as_ref());

    let mut g = [0u8; 32];
    let h = piece_hash(&e, &f);
    g.copy_from_slice(h.as_ref());
    let a = PieceInfo::new (a, UnpaddedBytesAmount(127));
    let b = PieceInfo::new (b, UnpaddedBytesAmount(127));
    let c = PieceInfo::new (c, UnpaddedBytesAmount(127));
    let d = PieceInfo::new (d, UnpaddedBytesAmount(127));

    let e = PieceInfo::new (e, UnpaddedBytesAmount(254));
    let f = PieceInfo::new (f, UnpaddedBytesAmount(254));
    let g = PieceInfo::new (g, UnpaddedBytesAmount(508));

    let sector_size = SectorSize(4 * 128);
    let comm_d = g.commitment;

    // println!("e: {:?}", e);
    // println!("f: {:?}", f);
    // println!("g: {:?}", g);

    BOOST_CHECK(
        verify_pieces(&comm_d, &[ a.clone(), b.clone(), c.clone(), d.clone() ], sector_size).expect("failed to verify"),
        "[a, b, c, d]");

    BOOST_CHECK(verify_pieces(&comm_d, &[ e.clone(), c, d ], sector_size).expect("failed to verify"), "[e, c, d]");

    BOOST_CHECK(verify_pieces(&comm_d, &[ e, f.clone() ], sector_size).expect("failed to verify"), "[e, f]");

    BOOST_CHECK(verify_pieces(&comm_d, &[ a, b, f ], sector_size).expect("failed to verify"), "[a, b, f]");

    BOOST_CHECK(verify_pieces(&comm_d, &[g], sector_size).expect("failed to verify"), "[g]");
}

BOOST_AUTO_TEST_CASE(test_verify_padded_pieces) {
    // [
    //   {(A0 00) (BB BB)} -> A(1) P(1) P(1) P(1) B(4)
    //   {(CC 00) (00 00)} -> C(2)      P(1) P(1) P(1) P(1) P(1) P(1)
    // ]
    // [
    //   {(DD DD) (DD DD)} -> D(8)
    //   {(00 00) (00 00)} -> P(1) P(1) P(1) P(1) P(1) P(1) P(1) P(1)
    // ]

    let sector_size = SectorSize(32 * 128);
    let pad = zero_padding(UnpaddedBytesAmount(127));

    let pieces = vec ![
        PieceInfo::new ([1u8; 32], UnpaddedBytesAmount(1 * 127)),
        PieceInfo::new ([2u8; 32], UnpaddedBytesAmount(4 * 127)),
        PieceInfo::new ([3u8; 32], UnpaddedBytesAmount(2 * 127)),
        PieceInfo::new ([4u8; 32], UnpaddedBytesAmount(8 * 127)),
    ];

    let padded_pieces = vec ![
        PieceInfo::new ([1u8; 32], UnpaddedBytesAmount(1 * 127)),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        PieceInfo::new ([2u8; 32], UnpaddedBytesAmount(4 * 127)),
        PieceInfo::new ([3u8; 32], UnpaddedBytesAmount(2 * 127)),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        PieceInfo::new ([4u8; 32], UnpaddedBytesAmount(8 * 127)),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad.clone(),
        pad,
    ];

    let hash = | a, b | {
        let hash = piece_hash(a, b);
        let mut res = [0u8; 32];
        res.copy_from_slice(hash.as_ref());
        res
    };

    std::array<std::uint8_t, 32> layer1 = {
        hash(&padded_pieces[0].commitment, &padded_pieces[1].commitment),      // 2: H(A(1) | P(1))
        hash(&padded_pieces[2].commitment, &padded_pieces[3].commitment),      // 2: H(P(1) | P(1))
        padded_pieces[4].commitment,                                           // 4: B(4)
        padded_pieces[5].commitment,                                           // 2: C(2)
        hash(&padded_pieces[6].commitment, &padded_pieces[7].commitment),      // 2: H(P(1) | P(1))
        hash(&padded_pieces[8].commitment, &padded_pieces[9].commitment),      // 2: H(P(1) | P(1))
        hash(&padded_pieces[10].commitment, &padded_pieces[11].commitment),    // 2: H(P(1) | P(1))
        padded_pieces[12].commitment,                                          // 8: D(8)
        hash(&padded_pieces[13].commitment, &padded_pieces[14].commitment),    // 2: H(P(1) | P(1))
        hash(&padded_pieces[15].commitment, &padded_pieces[16].commitment),    // 2: H(P(1) | P(1))
        hash(&padded_pieces[17].commitment, &padded_pieces[18].commitment),    // 2: H(P(1) | P(1))
        hash(&padded_pieces[19].commitment, &padded_pieces[20].commitment),    // 2: H(P(1) | P(1))
    };

    std::array<std::uint8_t, 32> layer2 = {
        hash(&layer1[0], &layer1[1]),      // 4
        layer1[2],                         // 4
        hash(&layer1[3], &layer1[4]),      // 4
        hash(&layer1[5], &layer1[6]),      // 4
        layer1[7],                         // 8
        hash(&layer1[8], &layer1[9]),      // 4
        hash(&layer1[10], &layer1[11]),    // 4
    };

    let layer3 = vec ![
        hash(&layer2[0], &layer2[1]),    // 8
        hash(&layer2[2], &layer2[3]),    // 8
        layer2[4],                       // 8
        hash(&layer2[5], &layer2[6]),    // 8
    ];

    let layer4 = vec ![
        hash(&layer3[0], &layer3[1]),    // 16
        hash(&layer3[2], &layer3[3]),    // 16
    ];

    let comm_d = hash(&layer4[0], &layer4[1]);    // 32

    BOOST_CHECK(verify_pieces(&comm_d, &pieces, sector_size));
}

BOOST_AUTO_TEST_CASE(test_verify_random_pieces) {
    let rng = &mut XorShiftRng::from_seed(crate::TEST_SEED);

    for
        sector_size in
            &[SectorSize(4 * 128), SectorSize(32 * 128), SectorSize(1024 * 128), SectorSize(1024 * 8 * 128), ] {
                println !("--- {:?} ---", sector_size);
    for (int i = 0; i < 100; i++) {
            println !(" - {} -", i);
            let unpadded_sector_size : UnpaddedBytesAmount = sector_size.clone().into();
            let sector_size = *sector_size;
            let padded_sector_size : PaddedBytesAmount = sector_size.into();

            let mut piece_sizes = Vec::new ();
            while(true) {
                std::size_t  sum_piece_sizes = sum_piece_bytes_with_alignment(piece_sizes);

                if (sum_piece_sizes
                    > padded_sector_size) {
                        piece_sizes.pop();
                        break;
                    }
                if (sum_piece_sizes
                    == padded_sector_size) {
                        break;
                    }

                while (true) {
                    // pieces must be power of two
                    let left = u64::from(padded_sector_size) - u64::from(sum_piece_sizes);
                let left_power_of_two = prev_power_of_two(left as u32);
                let max_exp = (left_power_of_two as f64).log2() as u32;

                std::size_t padded_exp;
                if (max_exp > 7) {
                    padded_exp = rng.gen_range(7,    // 2**7 == 128,
                                  max_exp);
                }
                else {padded_exp = 7;};
                let padded_piece_size = 2u64.pow(padded_exp);
                let piece_size : UnpaddedBytesAmount = PaddedBytesAmount(padded_piece_size).into();
                piece_sizes.push(piece_size);
                let sum : PaddedBytesAmount = sum_piece_bytes_with_alignment(&piece_sizes).into();

                if (sum
                    > padded_sector_size) {
                        // pieces might be too large after padding, so remove them and try again.
                        piece_sizes.pop();
                    }
                else {
                    break;
                }
            }
        }

    // println!(
    //     "  {:?}",
    //     piece_sizes
    //         .iter()
    //         .map(|s| u64::from(*s) / 127)
    //         .collect::<Vec<_>>()
    // );
    BOOST_CHECK(sum_piece_bytes_with_alignment(&piece_sizes) <= unpadded_sector_size);
    BOOST_CHECK(!piece_sizes.is_empty());

    let(comm_d, piece_infos) = build_sector(&piece_sizes, sector_size) ? ;

BOOST_CHECK(
verify_pieces(comm_d, piece_infos, sector_size));
            }
}
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <iostream>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <Eigen/Dense>
#include "grape.hpp"
#include "grapedb.hpp"
#include "lot.hpp"

namespace arbor
{

class Arbor
    {
    public:
                            Arbor();
                            ~Arbor();

        void                processCommandLineOptions(int argc, const char * argv[]);
        void                run();

    private:
        typedef std::pair<unsigned, unsigned>   size_topol_pair_t;
        typedef std::vector<double>             dlb_vect_t;
        typedef std::vector<double>             param_vect_t;
        typedef std::vector<param_vect_t>       sample_vect_t;
        typedef std::pair<unsigned, double>     pair_uint_dbl_t;
        typedef pair_uint_dbl_t &               pair_uint_dbl_ref_t;

        enum ParamType {ptype_edge_length, ptype_gamma_shape, ptype_pinvar, ptype_exchangeability, ptype_frequency};

        // member functions
        void                initRandom();
        void                showUserSpecifiedOptions() const;
        void                calcParamTypesFromColumnHeaders();
        void                readData();
        void                showParamTable() const;
        void                calcIndivTopolMargLikes();
        void                showTopoFreq();
        void                showTopoTable();
        double              summationWithFloatingControl(const std::vector<double> & v) const;
        void                margLikeOneTopology(sample_vect_t & parameters, dlb_vect_t & log_likelihoods, dlb_vect_t & log_priors);
        void                chooseGrapeSeeds(sample_vect_t & parameters);
        void                calcMeansAndStdevs(sample_vect_t & parameters);
        void                standardizeSampleAllowingCorrelation(sample_vect_t & parameters);
        void                standardizeSampleAssumingIndependence(sample_vect_t & parameters);
        double              calcDistance(std::vector<double> & a, std::vector<double> & b) const;
        double              calcPutativeRadius(unsigned index, sample_vect_t & parameters, unsigned num_to_keep, std::vector<pair_uint_dbl_t> & neighbors);
        void                createGrapes(sample_vect_t & parameters, dlb_vect_t & log_likelihoods, dlb_vect_t & log_priors, dlb_vect_t & log_jacobians);
        void                calcDeltaTerms(sample_vect_t & parameters, dlb_vect_t & log_likelihoods, dlb_vect_t & log_priors, dlb_vect_t & log_jacobians);
        void                calcRatioTerms(sample_vect_t & parameters, dlb_vect_t & log_likelihoods, dlb_vect_t & log_priors, dlb_vect_t & log_jacobians);
        void                calcOverallMargLike();

        // data members representing user-configurable options
        std::string                             _data_file_name;            // the name of the sample file to be processed
        unsigned                                _min_sample_size;           // minimum number of samples for tree topology inclusion
        unsigned                                _rnseed;                    // pseudorandom number seed
        double                                  _grape_fraction;            // fraction of sample used to define grapes

        // data members that could become user-configurable options in the future
        unsigned                                _skip;                      // number of samples to skip (as burn-in)
        bool                                    _correlation;               // if true, uses full covariance matrix to standardize parameters

        // other data members
        Lot::SharedPtr                          _lot;                       // Pseudorandom number generator
        GrapeDatabase                           _db;                        // object that stores results for each tree topology and used for outputting summaries of results
        std::vector<std::string>                _column_headers;            // labels in first row of parameter file
        std::map<unsigned, dlb_vect_t>          _log_likelihoods;           // map with key=topology and value=vector of log-likelihood (3rd column of parameter file)
        std::map<unsigned, dlb_vect_t>          _log_priors;                // map with key=topology and value=vector of log-prior (4th column of parameter file)
        std::map<unsigned, sample_vect_t>       _parameters;                // map with key=topology and value=vector of parameter vectors
        unsigned                                _nsubsets;                  // number of data subsets (determined from prefixes in column headers)
        unsigned                                _nparams;                   // length of each vector stored in _parameters
        std::vector<ParamType>                  _param_types;               // parameter types in order of appearance in sample vectors
        std::vector<unsigned>                   _param_subsets;             // data subset of each parameter in order of appearance in sample vectors
        std::vector<size_topol_pair_t>          _tree_topologies;           // vector storing topology number and frequency pairs
        unsigned                                _N;                         // total number of estimation sample points used over all tree topologies
        unsigned                                _total_sample_size;         // total number of samples used (does not count samples from tree topologies not used)
        double                                  _min_radius;                // minimum radius of all grapes for one particular tree topology
        std::vector<double>                     _log_ratio_terms;           // terms composing numerator of estimator of 1/c
        std::vector<double>                     _delta_terms;               // terms composing denominator of estimator of 1/c

        // - used if assuming independence among parameters
        std::vector<double>                     _means;                     // means of parameters from reference sample
        std::vector<double>                     _stdevs;                    // standard deviations of parameters from reference sample

        // - used if allowing correlations among parameters
        Eigen::VectorXd                         _Means;                     // 1 x p vector of column means
        Eigen::VectorXd                         _primary_eigenvector;       // eigenvector with largest eigenvalue from variance-covariance matrix
        Eigen::MatrixXd                         _Y;                         // n x p matrix of sampled parameter vectors
        Eigen::MatrixXd                         _M;                         // n x p matrix in which each row is vector of column means
        Eigen::MatrixXd                         _S;                         // p x p covariance matrix raised to power 0.5
        Eigen::MatrixXd                         _Sinv;                      // p x p covariance matrix raised to power -0.5
        double                                  _logdetS;                   // log |determinant of _S|

        // data members that are emptied and reused for each topology
        unsigned                                _in_multiple_grapes;        // number of estimation sample points placed in more than one grape
        std::vector<double>                     _placed;                    // vector of absolute differences between log-kernel of placed sample point and the log-kernel of the center of the grape in which it was placed
        std::vector<Grape>                      _grapes;                    // vector of Grape objects
        std::vector<double>                     _log_jacobians;             // log of the jacobian for log-transformation plus standardization
        std::vector<unsigned>                   _reference_indices;         // Indices into _parameters of reference sample points
        std::vector<unsigned>                   _estimation_indices;        // Indices into _parameters of estimation sample points

        // static data members
        static std::string                      _program_name;              // name of the program
        static std::string                      _author;                    // author of program
        static unsigned                         _major_version;             // major version number (e.g. "1" in "version 1.2")
        static unsigned                         _minor_version;             // minor version number (e.g. "2" in "version 1.2")
        static unsigned                         _def_rnseed;                // default value for _rnseed
        static unsigned                         _def_minsamplesize;         // default value for _min_sample_size
        static double                           _def_grapefraction;         // default value for _grape_fraction

    };

inline Arbor::Arbor()
  : _min_sample_size(_def_minsamplesize)
  , _rnseed(_def_rnseed)
  , _grape_fraction(_def_grapefraction)
  , _skip(1)
  , _correlation(true)
    {
    std::cout << boost::str(boost::format("%s %d.%d (written by %s)") % _program_name % _major_version % _minor_version % _author) << std::endl;
    }

inline Arbor::~Arbor()
    {
    }

}


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/fast-lstm.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <type_traits>
#include <sys/types.h>
#include <sys/stat.h>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/program_options.hpp>

#include "s2s/dynet/dict.h"
#include "s2s/corpus/options.hpp"
#include "s2s/corpus/corpora.hpp"
#include "s2s/corpus/dicts.hpp"
#include "s2s/corpus/batch.hpp"
#include "s2s/nn/encdec.hpp"

#ifndef INCLUDE_GUARD_ENCODER_DECODER_BASE_HPP
#define INCLUDE_GUARD_ENCODER_DECODER_BASE_HPP

namespace s2s {

class encoder_decoder_base : public encoder_decoder {

public:

    std::vector<dynet::Expression> h_bi;
    dynet::Expression i_feed;

    explicit encoder_decoder_base(dynet::ParameterCollection& model, const s2s_options& opts, dicts& d) : encoder_decoder(model,opts,d) {

        flag_drop_out = true;
        unsigned int dec_feeding_size = 0;

        assert(opts.dec_feature_vocab_size.size() == opts.dec_feature_vec_size.size());
        unsigned int cell_ratio = 2; // In GRU, cell_ratio = 1;

        p_dec_init_w.resize(opts.num_layers * cell_ratio);
        for(unsigned int i = 0; i < opts.num_layers * cell_ratio; i++){
            p_dec_init_w[i] = model.add_parameters({opts.rnn_size, opts.rnn_size});
        }
        p_dec_init_bias.resize(opts.num_layers * cell_ratio);
        for(unsigned int i = 0; i < opts.num_layers * cell_ratio; i++){
            p_dec_init_bias[i] = model.add_parameters({opts.rnn_size});
        }
        if(opts.additional_output_layer == true){
            dec_feeding_size = opts.rnn_size;
        }else{
            dec_feeding_size = opts.rnn_size * 3;
        }
        if(opts.additional_output_layer == true){
            p_add_W = model.add_parameters({dec_feeding_size, opts.rnn_size * 3});
            p_add_bias = model.add_parameters({dec_feeding_size});
            p_out_R = model.add_parameters({d.dict_set_trg.d_word.size(), opts.rnn_size});
            p_out_bias = model.add_parameters({d.dict_set_trg.d_word.size()});
        }else{
            p_out_R = model.add_parameters({d.dict_set_trg.d_word.size(), dec_feeding_size});
            p_out_bias = model.add_parameters({d.dict_set_trg.d_word.size()});
        }

        dec_builder = dynet::VanillaLSTMBuilder(
            opts.num_layers,
            (rev_enc_builder.input_dim + dec_feeding_size + 3),
            opts.rnn_size,
            model
        );

    }

    encoder_decoder_base(const encoder_decoder_base&) = delete;
    encoder_decoder_base& operator=(const encoder_decoder_base&) = delete;
    ~encoder_decoder_base() = default;

    void encoder(const batch &batch_local, dynet::ComputationGraph& cg) {
        // initialize
        init(batch_local, cg);
        // embedding
        embedding(batch_local, cg);

        std::vector<dynet::Expression> h_fwd(slen);
        std::vector<dynet::Expression> h_bwd(slen);
        h_bi.resize(slen);

        // forward encoder
        fwd_enc_builder.new_graph(cg);
        fwd_enc_builder.start_new_sequence();
        if(flag_drop_out == true && opts.dropout_rate_lstm_word > 0.f){
            fwd_enc_builder.set_dropout_masks(batch_size);
        }
        for (unsigned int t_i = 0; t_i < slen; ++t_i) {
            fwd_enc_builder.add_input(h_lookup[t_i]);
            h_fwd[t_i] = fwd_enc_builder.back();
        }
        // backward encoder
        rev_enc_builder.new_graph(cg);
        rev_enc_builder.start_new_sequence();
        if(flag_drop_out == true && opts.dropout_rate_lstm_word > 0.f){
            rev_enc_builder.set_dropout_masks(batch_size);
        }
        for (unsigned int ind = 0; ind < slen; ++ind) {
            unsigned int t_i = (slen - 1) - ind;
            rev_enc_builder.add_input(h_lookup[t_i]);
            h_bwd[t_i] = rev_enc_builder.back();
        }
        // bidirectional encoding
        for (unsigned int t_i = 0; t_i < slen; ++t_i) {
            h_bi[t_i] = concatenate(std::vector<dynet::Expression>({h_fwd[t_i], h_bwd[t_i]}));
        }
        // Initialize decoder
        std::vector<dynet::Expression> vec_enc_final_state;
        vec_enc_final_state = rev_enc_builder.final_s();
        dec_builder.new_graph(cg);
        if(opts.additional_connect_layer){
            std::vector<dynet::Expression> vec_dec_init_state;
            for (unsigned int i = 0; i < vec_enc_final_state.size(); i++){
                dynet::Expression i_dec_init_w = parameter(cg, p_dec_init_w[i]);
                dynet::Expression i_dec_init_bias = parameter(cg, p_dec_init_bias[i]);
                vec_dec_init_state.push_back(tanh(i_dec_init_w * vec_enc_final_state[i] + i_dec_init_bias));
            }
            dec_builder.start_new_sequence(vec_dec_init_state);
        }else{
            dec_builder.start_new_sequence(vec_enc_final_state);
        }
        if(flag_drop_out == true && opts.dropout_rate_lstm_word > 0.f){
            dec_builder.set_dropout_masks(batch_size);
        }
        init_feed(batch_local, cg);
    }

    void init_feed(const batch &batch_local, dynet::ComputationGraph& cg){
        i_feed = dynet::zeroes(cg, dynet::Dim({p_out_R.dim().d[1]}, batch_local.batch_size()));
    }

    dynet::Expression decoder_output(dynet::ComputationGraph& cg,  const std::vector<unsigned int> prev, const unsigned int t){
        return decoder_output(cg, prev, t, dec_builder.state());
    }

    dynet::Expression decoder_output(dynet::ComputationGraph& cg,  const std::vector<unsigned int> prev, const unsigned int t, const dynet::RNNPointer pointer_prev){
        // t should be always larger than 0.
        assert(t > 0);
        // convert previous label to the bit features.
        std::vector<dynet::real> bit_features;
        const unsigned int start_id = d.dict_set_trg.start_id_word;
        const unsigned int kep_id = d.dict_set_trg.keep_id_word;
        const unsigned int del_id = d.dict_set_trg.delete_id_word;
        for(unsigned int batch_id = 0; batch_id < prev.size(); batch_id++){
            // start
            if(prev[batch_id] == start_id){
                bit_features.push_back(opts.bit_size_flt);
            }else{
                bit_features.push_back(0.0f);
            }
            // keep
            if(prev[batch_id] == kep_id){
                bit_features.push_back(opts.bit_size_flt);
            }else{
                bit_features.push_back(0.0f);
            }
            // delete
            if(prev[batch_id] == del_id){
                bit_features.push_back(opts.bit_size_flt);
            }else{
                bit_features.push_back(0.0f);
            }
        }
        dynet::Expression i_x_t = dynet::input(cg, dynet::Dim({3}, batch_size), bit_features);
        dynet::Expression i_dec_input = concatenate(std::vector<dynet::Expression>({i_x_t, i_feed}));
        i_dec_input = concatenate(std::vector<dynet::Expression>({i_dec_input, h_lookup[t]}));
        dec_builder.add_input(pointer_prev, i_dec_input);
        dynet::Expression i_h_dec = dec_builder.h.back().back();
        if(opts.additional_output_layer){
            dynet::Expression i_add_W = parameter(cg, p_add_W);
            dynet::Expression i_add_bias = parameter(cg, p_add_bias);
            i_feed = tanh(i_add_W * concatenate(std::vector<dynet::Expression>({i_h_dec, h_bi[t]})) + i_add_bias);
        }else{
            i_feed = concatenate(std::vector<dynet::Expression>({i_h_dec, h_bi[t]}));
        }
        dynet::Expression i_out_R = parameter(cg, p_out_R);
        dynet::Expression i_out_bias = parameter(cg, p_out_bias);
        dynet::Expression i_out_pred_t = i_out_R * i_feed + i_out_bias;
        return i_out_pred_t;
    }

    void disable_dropout(){
        fwd_char_enc_builder.disable_dropout();
        rev_char_enc_builder.disable_dropout();
        fwd_enc_builder.disable_dropout();
        rev_enc_builder.disable_dropout();
        dec_builder.disable_dropout();
        flag_drop_out = false;
    }

    void enable_dropout(){
        fwd_char_enc_builder.set_dropout(opts.dropout_rate_lstm_char, 0.f);
        rev_char_enc_builder.set_dropout(opts.dropout_rate_lstm_char, 0.f);
        fwd_enc_builder.set_dropout(opts.dropout_rate_lstm_word, 0.f);
        rev_enc_builder.set_dropout(opts.dropout_rate_lstm_word, 0.f);
        dec_builder.set_dropout(opts.dropout_rate_lstm_word, 0.f);
        flag_drop_out = true;
    }

private:
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version) {
        ar & p_word_enc;
        ar & p_char_enc;
        ar & p_feat_enc;
        ar & p_dec_init_bias;
        ar & p_dec_init_w;
        ar & p_layer_w;
        ar & p_add_W;
        ar & p_add_bias;
        ar & p_out_R;
        ar & p_out_bias;
        ar & dec_builder;
        ar & rev_char_enc_builder;
        ar & fwd_char_enc_builder;
        ar & rev_enc_builder;
        ar & fwd_enc_builder;
    }

};

}

#endif


/* Copyright 2015 Google Inc. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

// Computing label Op

#include <stdio.h>
#include <cfloat>
#include <math.h>
#include <time.h>

#include "types.h"
#include "sampler2D.h"
#include "ransac.h"
#include "Hypothesis.h"
#include "detection.h"
#include <Eigen/Geometry>

#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"

using namespace tensorflow;
typedef Eigen::ThreadPoolDevice CPUDevice;

REGISTER_OP("Houghvoting")
    .Attr("T: {float, double}")
    .Attr("is_train: int")
    .Input("bottom_label: int32")
    .Input("bottom_vertex: T")
    .Input("bottom_extents: T")
    .Input("bottom_meta_data: T")
    .Input("bottom_gt: T")
    .Output("top_box: T")
    .Output("top_pose: T")
    .Output("top_target: T")
    .Output("top_weight: T");

REGISTER_OP("HoughvotingGrad")
    .Attr("T: {float, double}")
    .Input("bottom_label: int32")
    .Input("bottom_vertex: T")
    .Input("grad: T")
    .Output("output_label: T")
    .Output("output_vertex: T");

/**
 * @brief Data used in NLOpt callback loop.
 */
struct DataForOpt
{
  int imageWidth;
  int imageHeight;
  float rx, ry;
  cv::Rect bb2D;
  std::vector<cv::Point3f> bb3D;
  cv::Mat_<float> camMat;
};

void getLabels(const int* label_map, std::vector<std::vector<int>>& labels, std::vector<int>& object_ids, int width, int height, int num_classes, int minArea);
void getBb3Ds(const float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes);
inline bool samplePoint2D(jp::id_t objID, std::vector<cv::Point2f>& eyePts, std::vector<cv::Point2f>& objPts, std::vector<float>& distances, const cv::Point2f& pt2D, const float* vertmap, int width, int num_classes);
std::vector<TransHyp*> getWorkingQueue(std::map<jp::id_t, std::vector<TransHyp>>& hypMap, int maxIt);
inline float point2line(cv::Point2d x, cv::Point2f n, cv::Point2f p);
inline void countInliers2D(TransHyp& hyp, const float * vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes, int pixelBatch);
inline void updateHyp2D(TransHyp& hyp, int maxPixels);
inline void filterInliers2D(TransHyp& hyp, int maxInliers);
inline cv::Point2f getMode2D(jp::id_t objID, const cv::Point2f& pt, const float* vertmap, float & distance, int width, int num_classes);
static double optEnergy(const std::vector<double> &pose, std::vector<double> &grad, void *data);
double poseWithOpt(std::vector<double> & vec, DataForOpt data, int iterations);
void estimateCenter(const int* labelmap, const float* vertmap, std::vector<std::vector<cv::Point3f>> bb3Ds, int batch, int height, int width, int num_classes, int is_train,
  float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> >& outputs);
void compute_target_weight(int height, int width, float* target, float* weight, std::vector<std::vector<cv::Point3f>> bb3Ds, const float* poses_gt, int num_gt, int num_classes, float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> > outputs);
inline void compute_width_height(TransHyp& hyp, const float* vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes);

template <typename Device, typename T>
class HoughvotingOp : public OpKernel {
 public:
  explicit HoughvotingOp(OpKernelConstruction* context) : OpKernel(context) {
    // Get the pool height
    OP_REQUIRES_OK(context,
                   context->GetAttr("is_train", &is_train_));
    // Check that pooled_height is positive
    OP_REQUIRES(context, is_train_ >= 0,
                errors::InvalidArgument("Need is_train >= 0, got ",
                                        is_train_));
  }

  // bottom_label: (batch_size, height, width)
  // bottom_vertex: (batch_size, height, width, 3 * num_classes)
  // top_box: (num, 6) i.e., batch_index, cls, x1, y1, x2, y2
  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);
    const Tensor& bottom_extents = context->input(2);

    // format of the meta_data
    // intrinsic matrix: meta_data[0 ~ 8]
    // inverse intrinsic matrix: meta_data[9 ~ 17]
    // pose_world2live: meta_data[18 ~ 29]
    // pose_live2world: meta_data[30 ~ 41]
    // voxel step size: meta_data[42, 43, 44]
    // voxel min value: meta_data[45, 46, 47]
    const Tensor& bottom_meta_data = context->input(3);
    auto meta_data = bottom_meta_data.flat<T>();

    const Tensor& bottom_gt = context->input(4);
    const float* gt = bottom_gt.flat<float>().data();

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    // batch size
    int batch_size = bottom_label.dim_size(0);
    // height
    int height = bottom_label.dim_size(1);
    // width
    int width = bottom_label.dim_size(2);
    // num of classes
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;
    int num_meta_data = bottom_meta_data.dim_size(3);
    int num_gt = bottom_gt.dim_size(0);

    // for each image, run hough voting
    std::vector<cv::Vec<float, 13> > outputs;
    const float* extents = bottom_extents.flat<float>().data();

    // bb3Ds
    std::vector<std::vector<cv::Point3f>> bb3Ds;
    getBb3Ds(extents, bb3Ds, num_classes);

    int index_meta_data = 0;
    float fx, fy, px, py;
    for (int n = 0; n < batch_size; n++)
    {
      const int* labelmap = bottom_label.flat<int>().data() + n * height * width;
      const float* vertmap = bottom_vertex.flat<float>().data() + n * height * width * VERTEX_CHANNELS * num_classes;
      fx = meta_data(index_meta_data + 0);
      fy = meta_data(index_meta_data + 4);
      px = meta_data(index_meta_data + 2);
      py = meta_data(index_meta_data + 5);

      estimateCenter(labelmap, vertmap, bb3Ds, n, height, width, num_classes, is_train_, fx, fy, px, py, outputs);

      index_meta_data += num_meta_data;
    }

    if (outputs.size() == 0)
    {
      std::cout << "no detection" << std::endl;
      // add a dummy detection to the output
      cv::Vec<float, 13> roi;
      roi(0) = 0;
      roi(1) = -1;
      roi(2) = 0;
      roi(3) = 0;
      roi(4) = 1;
      roi(5) = 1;
      roi(6) = 1;
      outputs.push_back(roi);
    }

    // Create output tensors
    // top_box
    int dims[2];
    dims[0] = outputs.size();
    dims[1] = 6;
    TensorShape output_shape;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape);

    Tensor* top_box_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_box_tensor));
    float* top_box = top_box_tensor->template flat<float>().data();

    // top_pose
    dims[1] = 7;
    TensorShape output_shape_pose;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_pose);

    Tensor* top_pose_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_pose, &top_pose_tensor));
    float* top_pose = top_pose_tensor->template flat<float>().data();

    // top target
    dims[1] = 4 * num_classes;
    TensorShape output_shape_target;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_target);

    Tensor* top_target_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(2, output_shape_target, &top_target_tensor));
    float* top_target = top_target_tensor->template flat<float>().data();
    memset(top_target, 0, outputs.size() * 4 * num_classes *sizeof(T));

    // top weight
    Tensor* top_weight_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(3, output_shape_target, &top_weight_tensor));
    float* top_weight = top_weight_tensor->template flat<float>().data();
    memset(top_weight, 0, outputs.size() * 4 * num_classes *sizeof(T));

    for(int n = 0; n < outputs.size(); n++)
    {
      cv::Vec<float, 13> roi = outputs[n];

      for (int i = 0; i < 6; i++)
        top_box[n * 6 + i] = roi(i);

      for (int i = 0; i < 7; i++)
        top_pose[n * 7 + i] = roi(6 + i);
    }

    if (is_train_)
      compute_target_weight(height, width, top_target, top_weight, bb3Ds, gt, num_gt, num_classes, fx, fy, px, py, outputs);
  }
 private:
  int is_train_;
};

REGISTER_KERNEL_BUILDER(Name("Houghvoting").Device(DEVICE_CPU).TypeConstraint<float>("T"), HoughvotingOp<CPUDevice, float>);

// compute gradient
template <class Device, class T>
class HoughvotingGradOp : public OpKernel {
 public:
  explicit HoughvotingGradOp(OpKernelConstruction* context) : OpKernel(context) {
  }

  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    // batch size
    int batch_size = bottom_label.dim_size(0);
    // height
    int height = bottom_label.dim_size(1);
    // width
    int width = bottom_label.dim_size(2);
    // num of classes
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;

    // construct the output shape
    TensorShape output_shape = bottom_label.shape();
    Tensor* top_label_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_label_tensor));
    T* top_label = top_label_tensor->template flat<T>().data();
    memset(top_label, 0, batch_size * height * width * sizeof(T));

    TensorShape output_shape_1 = bottom_vertex.shape();
    Tensor* top_vertex_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_1, &top_vertex_tensor));
    T* top_vertex = top_vertex_tensor->template flat<T>().data();
    memset(top_vertex, 0, batch_size * height * width * 2 * num_classes *sizeof(T));
  }
};

REGISTER_KERNEL_BUILDER(Name("HoughvotingGrad").Device(DEVICE_CPU).TypeConstraint<float>("T"), HoughvotingGradOp<CPUDevice, float>);

// get label lists
void getLabels(const int* label_map, std::vector<std::vector<int>>& labels, std::vector<int>& object_ids, int width, int height, int num_classes, int minArea)
{
  for(int i = 0; i < num_classes; i++)
    labels.push_back( std::vector<int>() );

  // for each pixel
  #pragma omp parallel for
  for(int x = 0; x < width; x++)
  for(int y = 0; y < height; y++)
  {
    int label = label_map[y * width + x];
    labels[label].push_back(y * width + x);
  }

  for(int i = 1; i < num_classes; i++)
  {
    if (labels[i].size() > minArea)
    {
      object_ids.push_back(i);
    }
  }
}

// get 3D bounding boxes
void getBb3Ds(const float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes)
{
  // for each object
  for (int i = 1; i < num_classes; i++)
  {
    cv::Vec<float, 3> extent;
    extent(0) = extents[i * 3];
    extent(1) = extents[i * 3 + 1];
    extent(2) = extents[i * 3 + 2];

    bb3Ds.push_back(getBB3D(extent));
  }
}

inline cv::Point2f getMode2D(jp::id_t objID, const cv::Point2f& pt, const float* vertmap, float & distance, int width, int num_classes)
{
  int channel = VERTEX_CHANNELS * objID;
  int offset = channel + VERTEX_CHANNELS * num_classes * (pt.y * width + pt.x);

  jp::coord2_t mode;
  mode(0) = vertmap[offset];
  mode(1) = vertmap[offset + 1];
  distance = vertmap[offset + 2];

  return cv::Point2f(mode(0), mode(1));
}

inline bool samplePoint2D(jp::id_t objID, std::vector<cv::Point2f>& eyePts, std::vector<cv::Point2f>& objPts, std::vector<float>& distances, const cv::Point2f& pt2D, const float* vertmap, int width, int num_classes)
{
  float distance;
  cv::Point2f obj = getMode2D(objID, pt2D, vertmap, distance, width, num_classes); // read out object coordinate

  eyePts.push_back(pt2D);
  objPts.push_back(obj);
  distances.push_back(distance);

  if (distance < 0)
    return false;
  else
    return true;
}

/**
 * @brief Creates a list of pose hypothesis (potentially belonging to multiple objects) which still have to be processed (e.g. refined).
 *
 * The method includes all remaining hypotheses of an object if there is still more than one, or if there is only one remaining but it still needs to be refined.
 *
 * @param hypMap Map of object ID to a list of hypotheses for that object.
 * @param maxIt Each hypotheses should be at least this often refined.
 * @return std::vector< Ransac3D::TransHyp*, std::allocator< void > > List of hypotheses to be processed further.
*/
std::vector<TransHyp*> getWorkingQueue(std::map<jp::id_t, std::vector<TransHyp>>& hypMap, int maxIt, int is_train)
{
  std::vector<TransHyp*> workingQueue;

  if (is_train)
  {
    for(auto it = hypMap.begin(); it != hypMap.end(); it++)
    for(int h = 0; h < it->second.size(); h++)
      if(it->second[h].refSteps < maxIt)
        workingQueue.push_back(&(it->second[h]));
  }
  else
  {
    for(auto it = hypMap.begin(); it != hypMap.end(); it++)
    for(int h = 0; h < it->second.size(); h++)
      if(it->second.size() > 1 || it->second[h].refSteps < maxIt) //exclude a hypothesis if it is the only one remaining for an object and it has been refined enough already
        workingQueue.push_back(&(it->second[h]));
  }

  return workingQueue;
}

inline float point2line(cv::Point2d x, cv::Point2f n, cv::Point2f p)
{
  float n1 = -n.y;
  float n2 = n.x;
  float p1 = p.x;
  float p2 = p.y;
  float x1 = x.x;
  float x2 = x.y;

  return fabs(n1 * (x1 - p1) + n2 * (x2 - p2)) / sqrt(n1 * n1 + n2 * n2);
}

inline float angle_distance(cv::Point2f x, cv::Point2f n, cv::Point2f p)
{
  return n.dot(x - p);
}

inline void countInliers2D(TransHyp& hyp, const float * vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes, int pixelBatch)
{
  // reset data of last RANSAC iteration
  hyp.inlierPts2D.clear();
  hyp.inliers = 0;

  hyp.effPixels = 0; // num of pixels drawn
  hyp.maxPixels += pixelBatch; // max num of pixels to be drawn

  int maxPt = labels[hyp.objID].size(); // num of pixels of this class
  float successRate = hyp.maxPixels / (float) maxPt; // probability to accept a pixel

  std::mt19937 generator;
  std::negative_binomial_distribution<int> distribution(1, successRate); // lets you skip a number of pixels until you encounter the next pixel to accept

  for(unsigned ptIdx = 0; ptIdx < maxPt;)
  {
    int index = labels[hyp.objID][ptIdx];
    cv::Point2d pt2D(index % width, index / width);

    hyp.effPixels++;

    // read out object coordinate
    float distance;
    cv::Point2d obj = getMode2D(hyp.objID, pt2D, vertmap, distance, width, num_classes);

    // inlier check
    float d = cv::norm(hyp.center - pt2D);
    if(point2line(hyp.center, obj, pt2D) < inlierThreshold && angle_distance(hyp.center, obj, pt2D) > 0 && d < std::max(hyp.bb.width, hyp.bb.height))
    {
      hyp.inlierPts2D.push_back(std::pair<cv::Point2d, cv::Point2d>(obj, pt2D)); // store object coordinate - camera coordinate correspondence
      hyp.inliers++; // keep track of the number of inliers (correspondences might be thinned out for speed later)
    }

    // advance to the next accepted pixel
    if(successRate < 1)
      ptIdx += std::max(1, distribution(generator));
    else
      ptIdx++;
  }
}

inline void compute_width_height(TransHyp& hyp, const float* vertmap, const std::vector<std::vector<int>>& labels, float inlierThreshold, int width, int num_classes)
{
  float w = -1;
  float h = -1;
  int maxPt = labels[hyp.objID].size(); // num of pixels of this class

  for(unsigned ptIdx = 0; ptIdx < maxPt; ptIdx++)
  {
    int index = labels[hyp.objID][ptIdx];
    cv::Point2d pt2D(index % width, index / width);

    // read out object coordinate
    float distance;
    cv::Point2d obj = getMode2D(hyp.objID, pt2D, vertmap, distance, width, num_classes);

    // inlier check
    float d = cv::norm(hyp.center - pt2D);
    if(point2line(hyp.center, obj, pt2D) < inlierThreshold && angle_distance(hyp.center, obj, pt2D) > 0 && d < std::max(hyp.bb.width, hyp.bb.height))
    {
      float x = fabs(pt2D.x - hyp.center.x);
      float y = fabs(pt2D.y - hyp.center.y);
      if (x > w)
        w = x;
      if (y > h)
        h = y;
    }
  }
  hyp.width_ = 2 * w;
  hyp.height_ = 2 * h;
}

inline void updateHyp2D(TransHyp& hyp, int maxPixels)
{
  if(hyp.inlierPts2D.size() < 4) return;
  filterInliers2D(hyp, maxPixels); // limit the number of correspondences

  // data conversion
  cv::Point2d center = hyp.center;
  Hypothesis trans(center);

  // recalculate pose
  trans.calcCenter(hyp.inlierPts2D);
  hyp.center = trans.getCenter();
}

inline void filterInliers2D(TransHyp& hyp, int maxInliers)
{
  if(hyp.inlierPts2D.size() < maxInliers) return; // maximum number not reached, do nothing

  std::vector<std::pair<cv::Point2d, cv::Point2d>> inlierPts; // filtered list of inlier correspondences

  // select random correspondences to keep
  for(unsigned i = 0; i < maxInliers; i++)
  {
    int idx = irand(0, hyp.inlierPts2D.size());

    inlierPts.push_back(hyp.inlierPts2D[idx]);
  }

  hyp.inlierPts2D = inlierPts;
}

void estimateCenter(const int* labelmap, const float* vertmap, std::vector<std::vector<cv::Point3f>> bb3Ds, int batch, int height, int width, int num_classes, int is_train,
  float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> >& outputs)
{
  //set parameters, see documentation of GlobalProperties
  int maxIterations = 10000000;
  float minArea = 400; // a hypothesis covering less projected area (2D bounding box) can be discarded (too small to estimate anything reasonable)
  float minDist2D = 10;
  float inlierThreshold3D = 0.5;
  int ransacIterations;  // 256
  int poseIterations = 100;
  int preemptiveBatch;  // 1000
  int maxPixels;  // 1000
  int refIt;  // 8

  if (is_train)
  {
    ransacIterations = 256;
    preemptiveBatch = 100;
    maxPixels = 1000;
    refIt = 4;
  }
  else
  {
    ransacIterations = 256 * 1;
    preemptiveBatch = 100 * 1;
    maxPixels = 1000 * 1;
    refIt = 8;
  }

  // labels
  std::vector<std::vector<int>> labels;
  std::vector<int> object_ids;
  getLabels(labelmap, labels, object_ids, width, height, num_classes, minArea);

  // camera matrix
  cv::Mat_<float> camMat = cv::Mat_<float>::zeros(3, 3);
  camMat(0, 0) = fx;
  camMat(1, 1) = fy;
  camMat(2, 2) = 1.f;
  camMat(0, 2) = px;
  camMat(1, 2) = py;

  if (object_ids.size() == 0)
    return;

  int imageWidth = width;
  int imageHeight = height;

  // hold for each object a list of pose hypothesis, these are optimized until only one remains per object
  std::map<jp::id_t, std::vector<TransHyp>> hypMap;

  // sample initial pose hypotheses
  #pragma omp parallel for
  for(unsigned h = 0; h < ransacIterations; h++)
  for(unsigned i = 0; i < maxIterations; i++)
  {
    // camera coordinate - object coordinate correspondences
    std::vector<cv::Point2f> eyePts;
    std::vector<cv::Point2f> objPts;
    std::vector<float> distances;

    // sample first point and choose object ID
    jp::id_t objID = object_ids[irand(0, object_ids.size())];

    if(objID == 0) continue;

    int pindex = irand(0, labels[objID].size());
    int index = labels[objID][pindex];
    cv::Point2f pt1(index % width, index / width);

    // sample first correspondence
    if(!samplePoint2D(objID, eyePts, objPts, distances, pt1, vertmap, width, num_classes))
      continue;

    // sample other points in search radius, discard hypothesis if minimum distance constrains are violated
    pindex = irand(0, labels[objID].size());
    index = labels[objID][pindex];
    cv::Point2f pt2(index % width, index / width);

    if (cv::norm(pt1 - pt2) < minDist2D)
      continue;

    if(!samplePoint2D(objID, eyePts, objPts, distances, pt2, vertmap, width, num_classes))
      continue;

    // reconstruct
    std::vector<std::pair<cv::Point2d, cv::Point2d>> pts2D;
    float distance = 0;
    for(unsigned j = 0; j < eyePts.size(); j++)
    {
      pts2D.push_back(std::pair<cv::Point2d, cv::Point2d>(
      cv::Point2d(objPts[j].x, objPts[j].y),
      cv::Point2d(eyePts[j].x, eyePts[j].y)
      ));
      distance += distances[j];
    }
    distance /= distances.size();

    Hypothesis trans(pts2D);

    // center
    cv::Point2d center = trans.getCenter();
    int x = int(center.x);
    int y = int(center.y);
    if (num_classes > 2 && x >= 0 && x < width && y >= 0 && y < height)
    {
      if (labelmap[y * width + x] == 0)
        continue;
    }

    // create a hypothesis object to store meta data
    TransHyp hyp(objID, center);

    // estimate a projection
    cv::Mat tvec(3, 1, CV_64F);
    cv::Mat rvec(3, 1, CV_64F);
    for(int j = 0; j < 3; j++)
    {
      tvec.at<double>(j, 0) = 0;
      rvec.at<double>(j, 0) = 0;
    }
    tvec.at<double>(2, 0) = distance;
    jp::cv_trans_t pose(rvec, tvec);

    std::vector<cv::Point2f> bb2D;
    cv::projectPoints(bb3Ds[objID-1], pose.first, pose.second, camMat, cv::Mat(), bb2D);

    // get min-max of projected vertices
    int minX = 10000000;
    int maxX = -10000000;
    int minY = 10000000;
    int maxY = -10000000;

    for(unsigned j = 0; j < bb2D.size(); j++)
    {
	minX = std::min((float) minX, bb2D[j].x);
	minY = std::min((float) minY, bb2D[j].y);
	maxX = std::max((float) maxX, bb2D[j].x);
	maxY = std::max((float) maxY, bb2D[j].y);
    }
    hyp.bb = cv::Rect(0, 0, (maxX - minX + 1), (maxY - minY + 1));

    cv::Point2f c;
    c.x = center.x;
    c.y = center.y;
    if (cv::norm(pt1 - c) > std::max(hyp.bb.width, hyp.bb.height) || cv::norm(pt2 - c) > std::max(hyp.bb.width, hyp.bb.height))
      continue;

    #pragma omp critical
    {
      hypMap[objID].push_back(hyp);
    }

    break;
  }

  // create a list of all objects where hyptheses have been found
  std::vector<jp::id_t> objList;
  for(std::pair<jp::id_t, std::vector<TransHyp>> hypPair : hypMap)
  {
    objList.push_back(hypPair.first);
  }

  // create a working queue of all hypotheses to process
  std::vector<TransHyp*> workingQueue = getWorkingQueue(hypMap, refIt, is_train);

  // main preemptive RANSAC loop, it will stop if there is max one hypothesis per object remaining which has been refined a minimal number of times
  while(!workingQueue.empty())
  {
    // draw a batch of pixels and check for inliers, the number of pixels looked at is increased in each iteration
    #pragma omp parallel for
    for(int h = 0; h < workingQueue.size(); h++)
      countInliers2D(*(workingQueue[h]), vertmap, labels, inlierThreshold3D, width, num_classes, preemptiveBatch);

    // sort hypothesis according to inlier count and discard bad half
    #pragma omp parallel for
    for(unsigned o = 0; o < objList.size(); o++)
    {
      jp::id_t objID = objList[o];
      if(hypMap[objID].size() > 1)
      {
	std::sort(hypMap[objID].begin(), hypMap[objID].end());
	hypMap[objID].erase(hypMap[objID].begin() + hypMap[objID].size() / 2, hypMap[objID].end());
      }
    }
    workingQueue = getWorkingQueue(hypMap, refIt, is_train);

    // refine
    #pragma omp parallel for
    for(int h = 0; h < workingQueue.size(); h++)
    {
      updateHyp2D(*(workingQueue[h]), maxPixels);
      workingQueue[h]->refSteps++;
    }

    workingQueue = getWorkingQueue(hypMap, refIt, is_train);
  }

  #pragma omp parallel for
  for(auto it = hypMap.begin(); it != hypMap.end(); it++)
  for(int h = 0; h < it->second.size(); h++)
  {
    cv::Vec<float, 13> roi;
    roi(0) = batch;
    roi(1) = it->second[h].objID;

    // backproject the center
    cv::Point2d center = it->second[h].center;
    float rx = (center.x - px) / fx;
    float ry = (center.y - py) / fy;
    float distance = it->second[h].compute_distance(vertmap, num_classes, width);

    // initial pose
    std::vector<double> vec(6);
    vec[0] = 0.0;
    vec[1] = 0.0;
    vec[2] = 0.0;
    vec[3] = rx * distance;
    vec[4] = ry * distance;
    vec[5] = distance;

    // convert pose to our format
    cv::Mat tvec(3, 1, CV_64F);
    cv::Mat rvec(3, 1, CV_64F);

    for(int i = 0; i < 6; i++)
    {
      if(i > 2)
        tvec.at<double>(i-3, 0) = vec[i];
      else
        rvec.at<double>(i, 0) = vec[i];
    }

    jp::cv_trans_t trans(rvec, tvec);
    jp::jp_trans_t pose = jp::cv2our(trans);

    // convert to quarternion
    cv::Mat pose_t;
    cv::transpose(pose.first, pose_t);
    Eigen::Map<Eigen::Matrix3d> eigenT( (double*)pose_t.data );
    Eigen::Quaterniond quaternion(eigenT);

    compute_width_height(it->second[h], vertmap, labels, inlierThreshold3D, width, num_classes);
    float scale = 0.05;
    roi(2) = center.x - it->second[h].width_ * (0.5 + scale);
    roi(3) = center.y - it->second[h].height_ * (0.5 + scale);
    roi(4) = center.x + it->second[h].width_ * (0.5 + scale);
    roi(5) = center.y + it->second[h].height_ * (0.5 + scale);

    roi(6) = quaternion.w();
    roi(7) = quaternion.x();
    roi(8) = quaternion.y();
    roi(9) = quaternion.z();
    roi(10) = pose.second.x;
    roi(11) = pose.second.y;
    roi(12) = pose.second.z;

    /*
    std::cout << pose.first << std::endl;
    std::cout << eigenT << std::endl;
    std::cout << quaternion.w() << " " << quaternion.x() << " " << quaternion.y() << " " << quaternion.z() << std::endl;
    std::cout << pose.second << std::endl;

    std::cout << "Inliers: " << it->second[h].inliers;
    std::printf(" (Rate: %.1f\%)\n", it->second[h].getInlierRate() * 100);
    std::cout << "Refined " << it->second[h].refSteps << " times. " << std::endl;
    std::cout << "Center " << center << std::endl;
    std::cout << "Width: " << it->second[h].width_ << " Height: " << it->second[h].height_ << std::endl;
    std::cout << "---------------------------------------------------" << std::endl;
    std::cout << roi << std::endl;
    */

    outputs.push_back(roi);

    if (is_train)
    {
      // add jittering rois
      float x1 = roi(2);
      float y1 = roi(3);
      float x2 = roi(4);
      float y2 = roi(5);
      float ww = x2 - x1;
      float hh = y2 - y1;

      // (-1, -1)
      roi(2) = x1 - 0.05 * ww;
      roi(3) = y1 - 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (+1, -1)
      roi(2) = x1 + 0.05 * ww;
      roi(3) = y1 - 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (-1, +1)
      roi(2) = x1 - 0.05 * ww;
      roi(3) = y1 + 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (+1, +1)
      roi(2) = x1 + 0.05 * ww;
      roi(3) = y1 + 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (0, -1)
      roi(2) = x1;
      roi(3) = y1 - 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (-1, 0)
      roi(2) = x1 - 0.05 * ww;
      roi(3) = y1;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (0, +1)
      roi(2) = x1;
      roi(3) = y1 + 0.05 * hh;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);

      // (+1, 0)
      roi(2) = x1 + 0.05 * ww;
      roi(3) = y1;
      roi(4) = roi(2) + ww;
      roi(5) = roi(3) + hh;
      outputs.push_back(roi);
    }
  }
}

static double optEnergy(const std::vector<double> &pose, std::vector<double> &grad, void *data)
{
  DataForOpt* dataForOpt = (DataForOpt*) data;

  cv::Mat tvec(3, 1, CV_64F);
  cv::Mat rvec(3, 1, CV_64F);

  for(int i = 0; i < 6; i++)
  {
    if(i > 2)
      tvec.at<double>(i-3, 0) = pose[i];
    else
      rvec.at<double>(i, 0) = pose[i];
  }

  jp::cv_trans_t trans(rvec, tvec);

  // project the 3D bounding box according to the current pose
  cv::Rect bb2D = getBB2D(dataForOpt->imageWidth, dataForOpt->imageHeight, dataForOpt->bb3D, dataForOpt->camMat, trans);

  // compute IoU between boxes
  float energy = -1 * getIoU(bb2D, dataForOpt->bb2D);

  return energy;
}

double poseWithOpt(std::vector<double> & vec, DataForOpt data, int iterations)
{
  // set up optimization algorithm (gradient free)
  nlopt::opt opt(nlopt::LN_NELDERMEAD, 6);

  // set optimization bounds
  double rotRange = 180;
  rotRange *= PI / 180;
  double tRangeXY = 0.01;
  double tRangeZ = 0.01; // pose uncertainty is larger in Z direction

  std::vector<double> lb(6);
  lb[0] = vec[0]-rotRange; lb[1] = vec[1]-rotRange; lb[2] = vec[2]-rotRange;
  lb[3] = vec[3]-tRangeXY; lb[4] = vec[4]-tRangeXY; lb[5] = vec[5]-tRangeZ;
  opt.set_lower_bounds(lb);

  std::vector<double> ub(6);
  ub[0] = vec[0]+rotRange; ub[1] = vec[1]+rotRange; ub[2] = vec[2]+rotRange;
  ub[3] = vec[3]+tRangeXY; ub[4] = vec[4]+tRangeXY; ub[5] = vec[5]+tRangeZ;
  opt.set_upper_bounds(ub);

  // configure NLopt
  opt.set_min_objective(optEnergy, &data);
  opt.set_maxeval(iterations);

  // run optimization
  double energy;
  nlopt::result result = opt.optimize(vec, energy);

  // std::cout << "IoU after optimization: " << -energy << std::endl;

  return energy;
}

// compute the pose target and weight
void compute_target_weight(int height, int width, float* target, float* weight, std::vector<std::vector<cv::Point3f>> bb3Ds,
  const float* poses_gt, int num_gt, int num_classes, float fx, float fy, float px, float py, std::vector<cv::Vec<float, 13> > outputs)
{
  int num = outputs.size();
  float threshold = 0.2;

  // camera matrix
  cv::Mat_<float> camMat = cv::Mat_<float>::zeros(3, 3);
  camMat(0, 0) = fx;
  camMat(1, 1) = fy;
  camMat(2, 2) = 1.f;
  camMat(0, 2) = px;
  camMat(1, 2) = py;

  // compute the gt boxes
  std::vector<cv::Rect> bb2Ds_gt(num_gt);
  for (int i = 0; i < num_gt; i++)
  {
    Eigen::Quaternionf quaternion(poses_gt[i * 13 + 6], poses_gt[i * 13 + 7], poses_gt[i * 13 + 8], poses_gt[i * 13 + 9]);
    Eigen::Matrix3f rmatrix = quaternion.toRotationMatrix();
    cv::Mat rmat_trans = cv::Mat(3, 3, CV_32F, rmatrix.data());
    cv::Mat rmat;
    cv::transpose(rmat_trans, rmat);
    cv::Point3d tvec(poses_gt[i * 13 + 10], poses_gt[i * 13 + 11], poses_gt[i * 13 + 12]);
    jp::jp_trans_t pose(rmat, tvec);
    jp::cv_trans_t trans = jp::our2cv(pose);

    int objID = int(poses_gt[i * 13 + 1]);
    std::vector<cv::Point3f> bb3D = bb3Ds[objID-1];
    bb2Ds_gt[i] = getBB2D(width, height, bb3D, camMat, trans);
  }

  for (int i = 0; i < num; i++)
  {
    cv::Vec<float, 13> roi = outputs[i];
    int batch_id = int(roi(0));
    int class_id = int(roi(1));

    // find the gt index
    int gt_ind = -1;
    for (int j = 0; j < num_gt; j++)
    {
      int gt_batch = int(poses_gt[j * 13 + 0]);
      int gt_id = int(poses_gt[j * 13 + 1]);
      if(class_id == gt_id && batch_id == gt_batch)
      {
        gt_ind = j;
        break;
      }
    }

    if (gt_ind == -1)
      continue;

    // compute bounding box overlap
    float x1 = roi(2);
    float y1 = roi(3);
    float x2 = roi(4);
    float y2 = roi(5);
    cv::Rect bb2D(x1, y1, x2-x1, y2-y1);

    float overlap = getIoU(bb2D, bb2Ds_gt[gt_ind]);
    if (overlap < threshold)
      continue;

    target[i * 4 * num_classes + 4 * class_id + 0] = poses_gt[gt_ind * 13 + 6];
    target[i * 4 * num_classes + 4 * class_id + 1] = poses_gt[gt_ind * 13 + 7];
    target[i * 4 * num_classes + 4 * class_id + 2] = poses_gt[gt_ind * 13 + 8];
    target[i * 4 * num_classes + 4 * class_id + 3] = poses_gt[gt_ind * 13 + 9];

    weight[i * 4 * num_classes + 4 * class_id + 0] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 1] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 2] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 3] = 1;

  }
}


//
// Created by Murtuza Husain on 26/03/2018.
//

#include <iostream>
#include <goicpzSurfaceUtils.h>
#include <GlobalRegister.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
//#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>   // TicToc
#include <pcl/point_types.h>
#include <pcl/common/projection_matrix.h>
#include <SurfaceRegister.h>
#include <boost/timer.hpp>

using namespace goicpz;

int main (int argc, char** argv) {
    boost::timer timer;
    int sampleSize = 600;

    GlobalRegister rigid(1, 0.0001);
    rigid.loadMoving("/Users/murtuza/dev/Matlab/data/IPCAI2018Data/reform/InitialCT_top.ply",
                     "/Users/murtuza/dev/Matlab/data/IPCAI2018Data/reform/InitialCT_top.ply",
                     "/Users/murtuza/dev/Matlab/data/IPCAI2018Data/reform/InitialCT_boundary.ply");
    rigid.preProcessMoving(sampleSize);
    std::cout << timer.elapsed() << std::endl;
    timer.restart();

    rigid.loadTarget("/Users/murtuza/dev/Matlab/data/IPCAI2018Data/reform/PartialDeformed4.ply",
                     "/Users/murtuza/dev/Matlab/data/IPCAI2018Data/reform/PartialDeformed4_boundary.ply");
    rigid.processTarget(sampleSize);
    std::cout <<  timer.elapsed() << std::endl;
    timer.restart();

    rigid.buildCorrespondeces();
    std::cout <<  timer.elapsed() << std::endl;
    timer.restart();

    Eigen::MatrixXf W = rigid.buildAffinityMatrix(0.3);
    std::cout <<  timer.elapsed() << std::endl;
    timer.restart();

    pcl::IndicesPtr moving_correspondence(new std::vector<int>);
    pcl::IndicesPtr target_correspondence(new std::vector<int>);

    rigid.prune_correspondence(W, 0.4, moving_correspondence, target_correspondence);
    std::cout << "Prune " <<  timer.elapsed() << std::endl;
    timer.restart();

    std::cout << rigid.moving_features_idx->size() << std::endl;

    int x = 9;

    /*PointCloudT::Ptr res = rigid.transform(moving_correspondence, target_correspondence);
    std::cout << "Transform " <<  timer.elapsed() << std::endl;
    timer.restart();*/

    return 0;
}


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

#include <boost/simd/swar/functions/split_multiplies.hpp>
#include <boost/simd/sdk/meta/is_upgradable.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/mpl/not.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF         ( split_multiplies_
                                      , tag::cpu_
                                      , (A0)(A1)
                                      , (simd::meta::is_upgradable_to<A0,A1>)
                                      , ((generic_< arithmetic_<A0> >))
                                        ((generic_< arithmetic_<A0> >))
                                        ((generic_< arithmetic_<A1> >))
                                      )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1, A1 & a3) const
    {
      result_type a2;
      boost::simd::split_multiplies(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF         ( split_multiplies_, tag::cpu_
                                      , (A0)
                                      , ( simd::meta::is_upgradable<A0> )
                                      , (generic_< arithmetic_<A0> >)
                                        (generic_< arithmetic_<A0> >)
                                      )
  {
    typedef typename dispatch::meta::upgrade<A0>::type part;
    typedef std::pair<part,part>                       result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A0 const& a1) const
    {
      part first, second;
      boost::simd::split_multiplies( a0, a1, first, second );
      return result_type(first, second);
    }
  };
} } }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2014-2015 Samuel Debionne, Grenoble, France.

// This file was modified by Oracle on 2015-2020.
// Modifications copyright (c) 2015-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_SEGMENT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_SEGMENT_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/algorithms/dispatch/expand.hpp>

#include <boost/geometry/core/tags.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Box, typename Segment
>
struct expand
    <
        Box, Segment,
        box_tag, segment_tag
    >
{
    template <typename Strategy>
    static inline void apply(Box& box,
                             Segment const& segment,
                             Strategy const& strategy)
    {
        strategy.expand(box, segment).apply(box, segment);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_SEGMENT_HPP