// Copyright 2020 Apex.AI, 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.

/// \copyright Copyright 2020 Apex.AI, Inc.
/// All rights reserved.

#include <state_estimation_nodes/kalman_filter_wrapper.hpp>

#include <common/types.hpp>
#include <kalman_filter/esrcf.hpp>
#include <motion_model/constant_acceleration.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/time.hpp>
#include <state_estimation_nodes/measurement.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/LinearMath/Quaternion.h>

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

#include <limits>
#include <cstdint>
#include <algorithm>

namespace
{
constexpr auto kCovarianceMatrixRows = 6U;
constexpr auto kIndexX = 0U;
constexpr auto kIndexY = kCovarianceMatrixRows + 1U;
constexpr auto kCovarianceMatrixRowsSquared = kCovarianceMatrixRows * kCovarianceMatrixRows;
static_assert(
  std::tuple_size<
    geometry_msgs::msg::PoseWithCovariance::_covariance_type>::value ==
  kCovarianceMatrixRowsSquared, "We expect the covariance matrix to have 36 entries.");

/// Convert a chrono timepoint to ros time.
rclcpp::Time to_ros_time(const std::chrono::system_clock::time_point & time_point)
{
  using std::chrono::duration_cast;
  using std::chrono::nanoseconds;
  return rclcpp::Time{duration_cast<nanoseconds>(time_point.time_since_epoch()).count()};
}

}  // namespace

namespace autoware
{
namespace prediction
{

using motion::motion_model::ConstantAcceleration;
using common::types::float64_t;
using common::types::bool8_t;

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
void KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::reset(
  const VectorT<kNumOfStates> & state,
  const SquareMatrixT<kNumOfStates> & initial_covariance_chol,
  const MeasurementBasedTime & event_timestamp,
  const GlobalTime & time_of_event_occurance)
{
  m_ekf->reset(state, initial_covariance_chol);
  m_time_keeper = MeasurementBasedTimeKeeper{time_of_event_occurance, event_timestamp};
  m_ekf_initialized = true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<typename MeasurementT>
void KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::reset(
  const MeasurementT & measurement,
  const GlobalTime & time_of_event_occurance)
{
  reset(
    measurement.get_values_in_full_state(m_motion_model.get_state()),
    m_initial_covariance_factor,
    measurement.get_acquisition_time(),
    time_of_event_occurance);
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<TimeReferenceFrame kTimeReferenceFrame>
common::types::bool8_t  // We cannot use an alias here as Doxygen thinks its a different signature.
KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::temporal_update(
  const Time<kTimeReferenceFrame> & time_of_update)
{
  if (!is_initialized()) {return false;}
  const auto dt = m_time_keeper.time_since_last_temporal_update(time_of_update);
  if (dt <= std::chrono::nanoseconds{0LL}) {return false;}
  m_ekf->temporal_update(dt);
  m_time_keeper.increment_last_temporal_update_time(dt);
  return true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
// cppcheck-suppress syntaxError
template<typename MeasurementT>
common::types::bool8_t  // We cannot use an alias here as Doxygen thinks its a different signature.
KalmanFilterWrapper<MotionModelT, kNumOfStates, kProcessNoiseDim>::observation_update(
  const GlobalTime & global_time_of_message_received,
  const MeasurementT & measurement)
{
  if (!is_initialized()) {
    // TODO(igor): this is not strictly correct, but should be good enough. If we get an observation
    // and the filter is not set to any state, we reset it. In this case we assume that this
    // measurement actually is statefull (not purely differential) and we ignore the variance of
    // this measurement.
    reset(measurement, global_time_of_message_received);
    return true;
  }
  // TODO(igor): I am not sure this should be calling latest_timestamp() as if we had a prediction
  // step with a later timestamp than our measurement here we will discard the measurement. Should
  // be just compare to the latest measurement time stored in the time keeper?
  if (m_time_keeper.latest_timestamp() > measurement.get_acquisition_time()) {return false;}
  if (!temporal_update(measurement.get_acquisition_time())) {return false;}
  // TODO(igor): I see a couple of ways to check mahalanobis distance in case the measurement does
  // not cover the full state. Here I upscale it to the full state, copying the values of the
  // current state for ones missing in the observation. We can alternatively apply the H matrix and
  // only compute the distance in the measurement world. Don't really know which one is best here.
  if (!passes_mahalanobis_gate(
      measurement.get_values_in_full_state(m_motion_model.get_state()),
      m_motion_model.get_state(),
      m_ekf->get_covariance())) {return false;}
  m_ekf->observation_update(
    measurement.get_values(),
    MeasurementT::template get_observation_to_state_mapping<kNumOfStates>(),
    measurement.get_variances());
  m_time_keeper.update_with_measurement(global_time_of_message_received, measurement);
  return true;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
bool8_t KalmanFilterWrapper<MotionModelT, kNumOfStates,
  kProcessNoiseDim>::passes_mahalanobis_gate(
  const VectorT<kNumOfStates> & sample,
  const VectorT<kNumOfStates> & mean,
  const SquareMatrixT<kNumOfStates> & covariance_factor) const
{
  // This is equivalent to the squared Mahalanobis distance of the form: diff.T * C.inv() * diff
  // Instead of the covariance matrix C we have its lower-triangular factor L, such that C = L * L.T
  // squared_mahalanobis_distance = diff.T * C.inv() * diff
  // = diff.T * (L * L.T).inv() * diff
  // = diff.T * L.T.inv() * L.inv() * diff
  // = (L.inv() * diff).T * (L.inv() * diff)
  // this allows us to efficiently find the squared Mahalanobis distance using (L.inv() * diff),
  // which can be found as a solution to: L * x = diff.
  const auto diff = sample - mean;
  const auto squared_threshold = m_mahalanobis_threshold * m_mahalanobis_threshold;
  const auto x = covariance_factor.ldlt().solve(diff);
  return x.transpose() * x < squared_threshold;
}

template<typename MotionModelT, std::int32_t kNumOfStates, int32_t kProcessNoiseDim>
nav_msgs::msg::Odometry KalmanFilterWrapper<MotionModelT, kNumOfStates,
  kProcessNoiseDim>::get_state() const
{
  static_assert(
    sizeof(MotionModelT) == -1,
    "You have to have a specialization for get_state() function!");
  // We only throw here because otherwise the linter complaints there is no return value.
  throw std::runtime_error("You have to have a specialization for get_state() function!");
}

template<>
nav_msgs::msg::Odometry ConstantAccelerationFilter::get_state() const
{
  using std::chrono::duration_cast;
  using std::chrono::nanoseconds;
  nav_msgs::msg::Odometry msg{};
  if (!is_initialized()) {
    throw std::runtime_error("Filter not is_initialized, cannot get state.");
  }
  msg.header.stamp = rclcpp::Time{to_ros_time(m_time_keeper.latest_timestamp())};
  msg.header.frame_id = m_frame_id;
  // Fill state.
  msg.pose.pose.position.x =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::POSE_X]);
  msg.pose.pose.position.y =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::POSE_Y]);
  msg.twist.twist.linear.x =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::VELOCITY_X]);
  msg.twist.twist.linear.y =
    static_cast<float64_t>(m_motion_model[ConstantAcceleration::States::VELOCITY_Y]);

  const auto rotation_around_z = std::atan2(msg.twist.twist.linear.y, msg.twist.twist.linear.x);
  tf2::Quaternion rotation;
  rotation.setRPY(0.0, 0.0, rotation_around_z);
  msg.pose.pose.orientation = tf2::toMsg(rotation);

  // Fill covariances.
  const auto & covariance_factor = m_ekf->get_covariance();
  const auto covariance = covariance_factor * covariance_factor.transpose();
  msg.pose.covariance[kIndexX] = static_cast<double>(covariance(
      ConstantAcceleration::States::POSE_X, ConstantAcceleration::States::POSE_X));
  msg.pose.covariance[kIndexY] = static_cast<double>(covariance(
      ConstantAcceleration::States::POSE_Y, ConstantAcceleration::States::POSE_Y));
  msg.twist.covariance[kIndexX] = static_cast<double>(covariance(
      ConstantAcceleration::States::VELOCITY_X, ConstantAcceleration::States::VELOCITY_X));
  msg.twist.covariance[kIndexY] = static_cast<double>(covariance(
      ConstantAcceleration::States::VELOCITY_Y, ConstantAcceleration::States::VELOCITY_Y));
  // TODO(igor): do we need elements off the diagonal?
  return msg;
}

/// Excplicit class instantiation.
template class KalmanFilterWrapper<ConstantAcceleration, 6, 2>;

using MeasurementPose = Measurement<common::types::float32_t,
    ConstantAcceleration::States::POSE_X,
    ConstantAcceleration::States::POSE_Y>;

using MeasurementPoseAndSpeed = Measurement<common::types::float32_t,
    ConstantAcceleration::States::POSE_X,
    ConstantAcceleration::States::POSE_Y,
    ConstantAcceleration::States::VELOCITY_X,
    ConstantAcceleration::States::VELOCITY_Y>;

using MeasurementSpeed = Measurement<common::types::float32_t,
    ConstantAcceleration::States::VELOCITY_X,
    ConstantAcceleration::States::VELOCITY_Y>;

template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::observation_update<>(
  const GlobalTime &, const MeasurementPose &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::observation_update<>(
  const GlobalTime &, const MeasurementSpeed &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::observation_update<>(
  const GlobalTime &, const MeasurementPoseAndSpeed &);

template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::temporal_update<>(
  const GlobalTime &);
template bool8_t KalmanFilterWrapper<ConstantAcceleration, 6, 2>::temporal_update<>(
  const MeasurementBasedTime &);

}  // namespace prediction
}  // namespace autoware


#include <stralgo.h>
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

  static_assert(!strconcept::indexable<char>);
  static_assert(!strconcept::indexable<int>);
  static_assert(strconcept::indexable<char *>);
  static_assert(strconcept::indexable<std::basic_string_view<char>>);

using namespace std::literals::string_view_literals;

  static_assert( stralgo::isspace(' ') == true );
  static_assert( stralgo::isspace('\r') == true );
  static_assert( stralgo::isspace('\n') == true );
  static_assert( stralgo::isspace('\t') == true );
  static_assert( stralgo::isspace('r') == false );
  static_assert( stralgo::isspace('b') == false );
  static_assert( stralgo::isspace('a') == false );

  static_assert( stralgo::isdigit(' ') == false );
  static_assert( stralgo::isdigit('\r') == false );
  static_assert( stralgo::isdigit('\n') == false );
  static_assert( stralgo::isdigit('\t') == false );
  static_assert( stralgo::isdigit('r') == false );
  static_assert( stralgo::isdigit('B') == false );
  static_assert( stralgo::isdigit('a') == false );
  static_assert( stralgo::isdigit('0') == true );
  static_assert( stralgo::isdigit('3') == true );
  static_assert( stralgo::isdigit('9') == true );
  static_assert( stralgo::isdigit(L'1') == true );
  static_assert( stralgo::isdigit(L'0') == true );
  static_assert( stralgo::isdigit(L'3') == true );
  static_assert( stralgo::isdigit(L'9') == true );
  static_assert( stralgo::isdigit(L'1') == true );

  static_assert( stralgo::isxdigit(' ') == false );
  static_assert( stralgo::isxdigit('\r') == false );
  static_assert( stralgo::isxdigit('\n') == false );
  static_assert( stralgo::isxdigit('\t') == false );
  static_assert( stralgo::isxdigit('r') == false );
  static_assert( stralgo::isxdigit('A') == true );
  static_assert( stralgo::isxdigit('B') == true );
  static_assert( stralgo::isxdigit('F') == true );
  static_assert( stralgo::isxdigit('G') == false );
  static_assert( stralgo::isxdigit('a') == true );
  static_assert( stralgo::isxdigit('f') == true );
  static_assert( stralgo::isxdigit('g') == false );
  static_assert( stralgo::isxdigit('0') == true );
  static_assert( stralgo::isxdigit('3') == true );
  static_assert( stralgo::isxdigit('9') == true );
  static_assert( stralgo::isxdigit(L'1') == true );
  static_assert( stralgo::isxdigit(L'0') == true );
  static_assert( stralgo::isxdigit(L'3') == true );
  static_assert( stralgo::isxdigit(L'9') == true );
  static_assert( stralgo::isxdigit(L'1') == true );
  static_assert( stralgo::isxdigit(L' ') == false );
  static_assert( stralgo::isxdigit(L'\r') == false );
  static_assert( stralgo::isxdigit(L'\n') == false );
  static_assert( stralgo::isxdigit(L'\t') == false );
  static_assert( stralgo::isxdigit(L'r') == false );
  static_assert( stralgo::isxdigit(L'A') == true );
  static_assert( stralgo::isxdigit(L'B') == true );
  static_assert( stralgo::isxdigit(L'F') == true );
  static_assert( stralgo::isxdigit(L'G') == false );
  static_assert( stralgo::isxdigit(L'a') == true );
  static_assert( stralgo::isxdigit(L'f') == true );
  static_assert( stralgo::isxdigit(L'g') == false );
  static_assert( stralgo::isxdigit(L'0') == true );
  static_assert( stralgo::isxdigit(L'3') == true );
  static_assert( stralgo::isxdigit(L'9') == true );
  static_assert( stralgo::isxdigit(L'1') == true );

  static_assert( stralgo::is_number( "0"sv ) == true );
  static_assert( stralgo::is_number( " "sv ) == false );
  static_assert( stralgo::is_number( "avde"sv ) == false );
  static_assert( stralgo::is_number( "9838320"sv ) == true );
  static_assert( stralgo::is_number( "9838a320"sv ) == false );
  static_assert( stralgo::is_number( "9838.320"sv ) == false );
  static_assert( stralgo::is_number( "9838,320"sv ) == false );
  static_assert( stralgo::is_number( ""sv ) == true );
  static_assert( stralgo::is_number( "9,"sv ) == false );
  static_assert( stralgo::is_number( ".9838320"sv ) == false );

  static_assert( stralgo::is_number( L"0"sv ) == true );
  static_assert( stralgo::is_number( L"9838320"sv ) == true );
  static_assert( stralgo::is_number( L"9838a320"sv ) == false );
  static_assert( stralgo::is_number( L"9838.320"sv ) == false );
  static_assert( stralgo::is_number( L"9838,320"sv ) == false );
  static_assert( stralgo::is_number( L""sv ) == true );
  static_assert( stralgo::is_number( L"9,"sv ) == false );
  static_assert( stralgo::is_number( L".9838320"sv ) == false );

  static_assert( stralgo::is_hexnumber( "0"sv ) == true );
  static_assert( stralgo::is_hexnumber( " "sv ) == false );
  static_assert( stralgo::is_hexnumber( "avde"sv ) == false );
  static_assert( stralgo::is_hexnumber( "9838320"sv ) == true );
  static_assert( stralgo::is_hexnumber( "9838a320"sv ) == true );
  static_assert( stralgo::is_hexnumber( "9838.320"sv ) == false );
  static_assert( stralgo::is_hexnumber( "9838,320"sv ) == false );
  static_assert( stralgo::is_hexnumber( ""sv ) == true );
  static_assert( stralgo::is_hexnumber( "9,"sv ) == false );
  static_assert( stralgo::is_hexnumber( ".9838320"sv ) == false );
  static_assert( stralgo::is_hexnumber( "afcbef"sv ) == true );
  static_assert( stralgo::is_hexnumber( "ABCDEF"sv ) == true );
  static_assert( stralgo::is_hexnumber( "AaBCcDEF"sv ) == true );
  static_assert( stralgo::is_hexnumber( "ABCDEFg"sv ) == false );
  static_assert( stralgo::is_hexnumber( "1AB3CDEF"sv ) == true );

  static_assert( stralgo::is_hexnumber( L"0"sv ) == true );
  static_assert( stralgo::is_hexnumber( L"9838320"sv ) == true );
  static_assert( stralgo::is_hexnumber( L"9838a320"sv ) == true );
  static_assert( stralgo::is_hexnumber( L"9838.320"sv ) == false );
  static_assert( stralgo::is_hexnumber( L"9838,320"sv ) == false );
  static_assert( stralgo::is_hexnumber( L""sv ) == true );
  static_assert( stralgo::is_hexnumber( L"9,"sv ) == false );
  static_assert( stralgo::is_hexnumber( L".9838320"sv ) == false );
  static_assert( stralgo::is_hexnumber( L"afcbef"sv ) == true );
  static_assert( stralgo::is_hexnumber( L"ABCDEF"sv ) == true );
  static_assert( stralgo::is_hexnumber( L"AaBCcDEF"sv ) == true );
  static_assert( stralgo::is_hexnumber( L"ABCDEFg"sv ) == false );
  static_assert( stralgo::is_hexnumber( L"1AB3CDEF"sv ) == true );

//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_find_first_of
{
  auto constexpr t1a {"Lorem ipsum dolor sit amet, consectetur adipiscing elit. "sv};
  static_assert( stralgo::find_first_of(t1a, ""sv) == t1a.npos);
  static_assert( stralgo::find_first_of(t1a, " i"sv) == 5u);
  static_assert( stralgo::find_first_of(t1a, "i "sv) == 5u);
  static_assert( stralgo::find_first_of(t1a, "XZ"sv) == t1a.npos);
  static_assert( stralgo::find_first_of(std::string_view{}, "XZ"sv) == t1a.npos);

  auto constexpr t2a {L"Lorem ipsum dolor sit amet, consectetur adipiscing elit. "sv};
  static_assert( stralgo::find_first_of(t2a, L""sv) == t1a.npos);
  static_assert( stralgo::find_first_of(t2a, L" i"sv) == 5u);
  static_assert( stralgo::find_first_of(t2a, L"XZ"sv) == t1a.npos);
  static_assert( stralgo::find_first_of(std::wstring_view{}, L"XZ"sv) == t1a.npos);
}

//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_starts_with
  {
  auto constexpr t1a {" \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv};
  static_assert( stralgo::starts_with(t1a, ""sv) );
  static_assert( !stralgo::starts_with(t1a, "Lorem"sv) );
  static_assert( stralgo::starts_with(t1a, " \t\r\nLorem"sv) );

  auto constexpr t2a {L" \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv};
  static_assert( stralgo::starts_with(t2a, L""sv) );
  static_assert( !stralgo::starts_with(t2a, L"Lorem"sv) );
  static_assert( stralgo::starts_with(t2a, L" \t\r\nLorem"sv) );
  }

//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_trim_left
{
  constexpr auto t1a {" \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv};

  static_assert( stralgo::trim_left(t1a) == "Lorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv );
  static_assert( stralgo::trim_left(t1a,std::string_view{}) == t1a );
  static_assert( stralgo::trim_left(t1a, ' ') == "\t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv );
  static_assert( stralgo::trim_left(t1a, "Lorem  \t\r\n"sv) == "ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv );
}

//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_trim_right
{
  constexpr auto t1a {" \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv};

  static_assert( stralgo::trim_right(t1a) == " \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit."sv );
  static_assert( stralgo::trim_right(t1a,std::string_view{}) == t1a );
  static_assert( stralgo::trim_right(t1a,'\n') == " \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r"sv );
  static_assert( stralgo::trim_right(t1a,"Lorem  \t\r\nelit."sv) == " \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing"sv );
}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_trim
{

  constexpr auto t1a {" \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv};

    static_assert( stralgo::trim(t1a) == "Lorem ipsum dolor sit amet, consectetur adipiscing elit."sv );
    static_assert( std::is_same_v<decltype(stralgo::trim(t1a)),std::string_view> );
    static_assert( stralgo::trim(t1a,std::string_view{}) == t1a );
    static_assert( stralgo::trim(t1a,'\n') == " \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r"sv );
    static_assert( stralgo::trim(t1a,"Lorem  \t\r\nelit."sv) == "psum dolor sit amet, consectetur adipiscing"sv );
}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_ends_with
{
  auto constexpr t1a {" \t\r\nLorem ipsum dolor sit amet, consectetur adipiscing elit. \t\r\n"sv};
  static_assert( stralgo::ends_with(t1a, ""sv) == true );
  static_assert( stralgo::ends_with(t1a, std::string_view{}) == true );
  static_assert( stralgo::ends_with(t1a, "elit."sv) == false );
  static_assert( stralgo::ends_with(t1a, "elit. \t\r\n"sv) == true );
}
//----------------------------------------------------------------------------------------------------------------------

namespace stralgo_compare_no_case
{

  auto constexpr t1 {"Lorem ipsum"sv}; auto constexpr t1b {"Lorem ipsum"sv};
  auto constexpr t2 {"Lorem"sv};
  std::string_view constexpr t4 {};
  static_assert( stralgo::compare_no_case(t1,t2) == 1 );
  static_assert( stralgo::compare_no_case(t1,t1b) == 0 );
  static_assert( stralgo::compare_no_case(t1,t1) == 0 );
  static_assert( stralgo::compare_no_case(t2,t1) == -1 );
  static_assert( stralgo::compare_no_case(t4,t4) == 0 );
  static_assert( stralgo::compare_no_case(t1,t4) == 1 );
  static_assert( stralgo::compare_no_case(t4,t1) == -1 );
}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_right
{
  constexpr auto t1 {"Lorem ipsum"sv};
  static_assert( stralgo::right( t1, 2 ) == "um"sv );
  static_assert( stralgo::right( t1, 0 ) == std::string_view{} );
  static_assert( stralgo::right( t1,  std::string_view::npos ) == t1 );
}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_left
{

  constexpr auto t1 {"Lorem ipsum"sv};
  static_assert( stralgo::left( t1, 2 ) == "Lo"sv );
  static_assert( stralgo::left( t1, 0 ) == std::string_view{} );
  static_assert( stralgo::left( t1, std::string_view::npos ) == t1 );

}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_substr
{

  constexpr auto t1 {"Lorem ipsum"sv};
  static_assert( stralgo::substr( t1, 2, 2 ) == "re"sv );
  static_assert( stralgo::substr( t1, 22, 2 ).empty() );
  static_assert( stralgo::substr( t1, 2, std::string_view::npos ) == "rem ipsum"sv );
  static_assert( stralgo::substr( t1, 0, std::string_view::npos ) == t1 );
  static_assert( stralgo::substr( t1, std::string_view::npos, 1 ).empty() );
  static_assert( stralgo::substr( t1, std::string_view::npos, std::string_view::npos ).empty() );
}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_to_lower
{
  static_assert( stralgo::to_lower('a') == 'a' );
  static_assert( stralgo::to_lower('A') == 'a' );
  static_assert( stralgo::to_lower('Z') == 'z' );
  static_assert( stralgo::to_lower('1') == '1' );
  static_assert( stralgo::to_lower(':') == ':' );
}
//----------------------------------------------------------------------------------------------------------------------
namespace stralgo_to_upper
{
  static_assert( stralgo::to_upper('a') == 'A' );
  static_assert( stralgo::to_upper('A') == 'A' );
  static_assert( stralgo::to_upper('z') == 'Z' );
  static_assert( stralgo::to_upper('1') == '1' );
  static_assert( stralgo::to_upper(':') == ':' );
}
//----------------------------------------------------------------------------------------------------------------------
struct stralgo_fixture_t {};

BOOST_FIXTURE_TEST_SUITE(stralgo_fixture, stralgo_fixture_t )

using std::string_view;
using std::wstring_view;

BOOST_AUTO_TEST_CASE(stralgo_merge)
{
  {
  string_view t1a {"orem ipsum dolor sit amet, consectetur adipiscing elit. "sv}, t2a { }, t3a {"Vestibulum"sv }; char t4a{' '}; string_view t5a{"rutrum leo libero"};
  constexpr string_view expected{"Lorem ipsum dolor sit amet, consectetur adipiscing elit. Vestibulum rutrum leo libero"};
  auto resa{ stralgo::merge('L',t1a,t2a,t3a,t4a,t5a)};

  BOOST_TEST( resa.compare(expected) == 0 );
  }
  {
  wstring_view t1a {L"orem ipsum dolor sit amet, consectetur adipiscing elit. "sv}, t2a { }, t3a {L"Vestibulum"sv }; wchar_t t4a{L' '};wstring_view t5a{L"rutrum leo libero"};
  constexpr wstring_view expected{L"Lorem ipsum dolor sit amet, consectetur adipiscing elit. Vestibulum rutrum leo libero"};
  auto resa{ stralgo::merge(L'L',t1a,t2a,t3a,t4a,t5a)};

  BOOST_TEST( resa.compare(expected) == 0 );
  }
}

//----------------------------------------------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(stralgo_merge_range)
{
  auto tst1 = { "Lorem ipsum"sv, "Lorem ipsum dolor "sv,"sit amet"sv, std::string_view{},""sv,", consectetur adipiscing elit."sv };
  auto res { stralgo::merge_range(std::begin(tst1), std::end(tst1) ) };
  BOOST_TEST( "Lorem ipsumLorem ipsum dolor sit amet, consectetur adipiscing elit."sv == res );

}
//----------------------------------------------------------------------------------------------------------------------
BOOST_AUTO_TEST_SUITE_END()


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

#include "./config.hpp"

#if !defined(BOOST_HASH_TEST_EXTENSIONS)

int main() {}

#else

#ifdef BOOST_HASH_TEST_STD_INCLUDES
#  include <functional>
#else
#  include <boost/functional/hash.hpp>
#endif

#include <boost/detail/lightweight_test.hpp>

#if defined(BOOST_MSVC)
#pragma warning(disable:4244)   // conversion from 'unsigned long' to
                                // 'unsigned short', possible loss of data
#pragma warning(disable:4245)   // conversion from 'int' to
                                // 'const unsigned short',
                                // signed/unsigned mismatch
#pragma warning(disable:4305)   // truncation from 'double' to
                                // 'const std::complex<float>::_Ty'
#pragma warning(disable:4309)   // truncation of constant value
#pragma warning(disable:4512)   // assignment operator could not be generated
#if BOOST_MSVC < 1400
#pragma warning(disable:4267)   // conversion from 'size_t' to 'unsigned int',
                                // possible loss of data
#endif
#endif

#if defined(__GNUC__) && !defined(BOOST_INTEL_CXX_VERSION)
#pragma GCC diagnostic ignored "-Wfloat-equal"
#endif

#include <complex>
#include <sstream>
#include <boost/limits.hpp>

template <class T>
void generic_complex_tests(std::complex<T> v)
{
    BOOST_HASH_TEST_NAMESPACE::hash<std::complex<T> > complex_hasher;

    BOOST_TEST(complex_hasher(v) == complex_hasher(v));

    BOOST_HASH_TEST_NAMESPACE::hash<T> real_hasher;
    T real = v.real();
    T imag = v.imag();

    BOOST_TEST(real_hasher(real) == complex_hasher(std::complex<T>(real)));

    if(imag != 0 && real_hasher(real) == complex_hasher(v)) {
        std::ostringstream os;
        os<<"real_hasher("<<real<<") == complex_hasher("
            <<v.real()<<" + "<<v.imag()<<"i) == "
            <<real_hasher(real)<<" (This might not be a bug).";
        BOOST_ERROR(os.str().c_str());
    }
}

template <class Float>
void complex_float_tests(Float*)
{
    typedef std::complex<Float> complex;
    generic_complex_tests(complex(0,0));
    generic_complex_tests(complex(0.5,0));
    generic_complex_tests(complex(25,0));
    generic_complex_tests(complex(25,0));
    generic_complex_tests(complex(static_cast<Float>(-67.5324535),static_cast<Float>(56.23578678)));
}

template <class Integer>
void complex_integral_tests(Integer*)
{
    typedef std::complex<Integer> complex;
    generic_complex_tests(complex(0,0));
    generic_complex_tests(complex(15342,124));
    generic_complex_tests(complex(25,54356));
    generic_complex_tests(complex(5325,2346));
    generic_complex_tests(complex(-243897,-49923874));
    generic_complex_tests(complex(-543,763));
}

int main()
{
    // I've comments out the short and unsigned short tests
    // as they cause warnings and don't really test
    // anything that the other tests already deal with.

    complex_float_tests((float*) 0);
    complex_float_tests((double*) 0);
    complex_float_tests((long double*) 0);
    //complex_integral_tests((short*) 0);
    complex_integral_tests((int*) 0);
    complex_integral_tests((long*) 0);
    //complex_integral_tests((unsigned short*) 0);
    complex_integral_tests((unsigned int*) 0);
    complex_integral_tests((unsigned long*) 0);

    return boost::report_errors();
}

#endif // BOOST_HASH_TEST_EXTENSIONS


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

#include <Eigen/Core>
#include <string>
#include <vector>

#include "common/types/object.hpp"
#include "object_builders/base_object_builder.hpp"

namespace autosense {
namespace object_builder {
/**
 * @brief Apollo doc
 *  盒构建器将恢复给定多边形点的完整边界框, 即使点云稀疏
 *  主要目的还是预估障碍物（例如，车辆）的方向
 */
class MinBoxObjectBuilder : public BaseObjectBuilder {
 public:
    MinBoxObjectBuilder() {}

    virtual ~MinBoxObjectBuilder() {}

    virtual void build(const std::vector<PointICloudPtr> &cloud_clusters,
                       std::vector<ObjectPtr> *objects);

    virtual void build(ObjectPtr object);

    virtual std::string name() const { return "MinBoxObjectBuilder"; }

 protected:
    bool buildObjects(std::vector<ObjectPtr> *objects);

    void buildObject(ObjectPtr object);

    void setDefaultValue(PointICloudPtr cloud,
                         ObjectPtr obj,
                         Eigen::Vector4f *min_pt,
                         Eigen::Vector4f *max_pt);

    void computeGeometricFeature(ObjectPtr obj);

    void computePolygon2dxy(ObjectPtr obj);

    void reconstructPolygon3d(ObjectPtr obj);

    double computeAreaAlongOneEdge(ObjectPtr obj,
                                   size_t first_in_point,
                                   Eigen::Vector3d *center,
                                   double *length,
                                   double *width,
                                   Eigen::Vector3d *dir);
};

}  // namespace object_builder
}  // namespace autosense

#endif  // OBJECT_BUILDERS_INCLUDE_OBJECT_BUILDERS_MIN_BOX_OBJECT_BUILDER_HPP_


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2015 Bryce Adelstein Lelbach aka wash
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
////////////////////////////////////////////////////////////////////////////////

#include <mdspan>

#include <boost/core/lightweight_test.hpp>

using std::rank;
using std::extent;

using std::experimental::dyn;
using std::experimental::dimensions;

int main()
{
    BOOST_TEST_EQ((rank<dimensions<       > >::value), 0);
    BOOST_TEST_EQ((rank<dimensions<3      > >::value), 1);
    BOOST_TEST_EQ((rank<dimensions<3, 5   > >::value), 2);
    BOOST_TEST_EQ((rank<dimensions<3, 5, 9> >::value), 3);

    BOOST_TEST_EQ((dimensions<       >::rank()), 0);
    BOOST_TEST_EQ((dimensions<3      >::rank()), 1);
    BOOST_TEST_EQ((dimensions<3, 5   >::rank()), 2);
    BOOST_TEST_EQ((dimensions<3, 5, 9>::rank()), 3);

    BOOST_TEST_EQ((extent<dimensions<>   >::value), 0);
    BOOST_TEST_EQ((extent<dimensions<>, 0>::value), 0);
    BOOST_TEST_EQ((extent<dimensions<>, 1>::value), 0);
    BOOST_TEST_EQ((extent<dimensions<>, 2>::value), 0);

    BOOST_TEST_EQ((extent<dimensions<3>   >::value), 3);
    BOOST_TEST_EQ((extent<dimensions<3>, 0>::value), 3);
    BOOST_TEST_EQ((extent<dimensions<3>, 1>::value), 0);

    BOOST_TEST_EQ((extent<dimensions<0>   >::value), 0);
    BOOST_TEST_NE((extent<dimensions<0>   >::value), dyn);
    BOOST_TEST_EQ((extent<dimensions<0>, 0>::value), 0);
    BOOST_TEST_NE((extent<dimensions<0>, 0>::value), dyn);
    BOOST_TEST_EQ((extent<dimensions<0>, 1>::value), 0);
    BOOST_TEST_NE((extent<dimensions<0>, 1>::value), dyn);

    BOOST_TEST_EQ((extent<dimensions<3, 5>   >::value), 3);
    BOOST_TEST_EQ((extent<dimensions<3, 5>, 0>::value), 3);
    BOOST_TEST_EQ((extent<dimensions<3, 5>, 1>::value), 5);
    BOOST_TEST_EQ((extent<dimensions<3, 5>, 2>::value), 0);

    BOOST_TEST_EQ((extent<dimensions<dyn, 5>   >::value), dyn);
    BOOST_TEST_EQ((extent<dimensions<dyn, 5>, 0>::value), dyn);
    BOOST_TEST_NE((extent<dimensions<dyn, 5>   >::value), 0);
    BOOST_TEST_NE((extent<dimensions<dyn, 5>, 0>::value), 0);
    BOOST_TEST_EQ((extent<dimensions<dyn, 5>, 1>::value), 5);
    BOOST_TEST_EQ((extent<dimensions<dyn, 5>, 2>::value), 0);

    BOOST_TEST_EQ((sizeof(dimensions<       >)), 1);
    BOOST_TEST_EQ((sizeof(dimensions<3      >)), 1);
    BOOST_TEST_EQ((sizeof(dimensions<3, 5   >)), 1);
    BOOST_TEST_EQ((sizeof(dimensions<3, 5, 9>)), 1);

    BOOST_TEST_EQ((sizeof(dimensions<dyn          >)), 1*sizeof(std::size_t));
    BOOST_TEST_EQ((sizeof(dimensions<dyn, dyn     >)), 2*sizeof(std::size_t));
    BOOST_TEST_EQ((sizeof(dimensions<dyn, dyn, dyn>)), 3*sizeof(std::size_t));

    BOOST_TEST_EQ((sizeof(dimensions<dyn, 5,   9  >)), 1*sizeof(std::size_t));
    BOOST_TEST_EQ((sizeof(dimensions<3,   dyn, 9  >)), 1*sizeof(std::size_t));
    BOOST_TEST_EQ((sizeof(dimensions<3,   5,   dyn>)), 1*sizeof(std::size_t));

    BOOST_TEST_EQ((sizeof(dimensions<3,   dyn, dyn>)), 2*sizeof(std::size_t));
    BOOST_TEST_EQ((sizeof(dimensions<dyn, 5,   dyn>)), 2*sizeof(std::size_t));
    BOOST_TEST_EQ((sizeof(dimensions<dyn, dyn, 9  >)), 2*sizeof(std::size_t));

    return boost::report_errors();
}


#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/program_options.hpp>
#include <glog/logging.h>
#include "colorize.h"

// Lab: L in [0, 100]
//      a in [-127, 127]
//      b in [-127, 127]

using namespace std;

void stat_ab (cv::Mat *stat, cv::Mat Lab, float ab_step) {
    CHECK(Lab.type() == CV_32FC3);
    for (int i = 0; i < Lab.rows; ++i) {
        float const *row = Lab.ptr<float const>(i);
        for (int j = 0; j < Lab.cols; ++j) {
            float l = row[0];
            float a = row[1];
            float b = row[2];
            row += 3;
            CHECK(l >= 0 && l <= 100);
            CHECK(a >= AB_MIN && a <= AB_MAX);
            CHECK(b >= AB_MIN && b <= AB_MAX);

            int ia = (a - AB_MIN) / ab_step;
            int ib = (b - AB_MIN) / ab_step;
            stat->at<float>(ia, ib) += 1.0;
        }
    }
}

int main (int argc, char *argv[]) {
    float ab_step;
    {
        namespace po = boost::program_options;
        po::options_description desc_visible("Allowed options");
        desc_visible.add_options()
            ("help,h", "produce help message.")
            ("ab_step", po::value(&ab_step)->default_value(10), "")
            ;

        po::options_description desc_hidden("Allowed options");
        desc_hidden.add_options()
            ;

        po::options_description desc("Allowed options");
        desc.add(desc_visible).add(desc_hidden);

        po::positional_options_description p;
        //p.add("input", 1);

        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).
                         options(desc).positional(p).run(), vm);
        po::notify(vm);

        if (vm.count("help")) {
            cout << "Usage:" << endl;
            cout << desc_visible;
            cout << endl;
            return 0;
        }
    }
    string path;

    int N = (AB_MAX - AB_MIN) / ab_step + 1;

    cv::Mat stat(N, N, CV_32F, cv::Scalar(0));  // a x b

    unsigned C = 0;
    while (cin >> path) {
        cv::Mat image = cv::imread(path, CV_LOAD_IMAGE_COLOR);
        cv::Mat image32;
        image.convertTo(image32, CV_32FC3);
        image32 *= 1.0/255;
        cv::Mat Lab;
        cv::cvtColor(image32, Lab, CV_BGR2Lab);
        stat_ab(&stat, Lab, ab_step);
        C += Lab.total();
    }
    if (C == 0) {
        cerr << "Using random RGB image." << endl;
        cv::Mat image32(800, 800, CV_32FC3);
        cv::randu(image32, cv::Scalar(0,0,0), cv::Scalar(256, 256, 256));
        image32 *= 1.0/255;
        cv::Mat Lab;
        cv::cvtColor(image32, Lab, CV_BGR2Lab);
        stat_ab(&stat, Lab, ab_step);
    }
    cv::normalize(stat, stat, 0, 255, cv::NORM_MINMAX);
    cv::imwrite("a.png", stat);
    return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, int N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N];
  T b[N], c[N];
  std::size_t sh1 =  sizeof(T)*8-1;

  int sh2 =  1;

  for(int i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::shift_right(a1[i], sh1);
    c[i] = bs::shift_right(a1[i], sh2);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  p_t cc(&c[0], &c[0]+N);

  STF_EQUAL(bs::shift_right(aa1, sh1), bb);
  STF_EQUAL(bs::shift_right(aa1, sh2), cc);
  STF_EQUAL((aa1 >>  sh1), bb);
  STF_EQUAL((aa1 >> sh2), cc);
}

STF_CASE_TPL("Check shift_right on pack with integral shift" , STF_INTEGRAL_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

template <typename T, int N, typename Env>
void tests(Env& $)
{
  using iT =  bd::as_integer_t<T>;
  using p_t = bs::pack<T, N>;
  using i_t = bs::pack<iT, N>;

  T a1[N], b[N];
  iT sh[N];

  for(int i = 0; i < N; ++i)
  {
    sh[i] = i%(8*sizeof(T));
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::shift_right(a1[i],sh[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  i_t sh1(&sh[0], &sh[0]+N);
  STF_EQUAL(bs::shift_right(aa1, sh1), bb);
  STF_EQUAL((aa1 >> sh1), bb);
}

STF_CASE_TPL("Check shift_right on pack with pack shift" , STF_INTEGRAL_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  tests<T, N>($);
  tests<T, N/2>($);
  tests<T, N*2>($);
}

STF_CASE_TPL (" shift_right integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using p_t = bs::pack<T>;

  using bs::shift_right;
  using ip_t = bd::as_integer_t<p_t>;
  using r_t = decltype(shift_right(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(shift_right(bs::One<p_t>(),bs::One<ip_t>()), bs::Zero<r_t>());
  STF_EQUAL(shift_right(bs::One<p_t>(),bs::Zero<ip_t>()), bs::One<r_t>());
  STF_EQUAL(shift_right(bs::Zero<p_t>(),bs::One<ip_t>()), bs::Zero<r_t>());
} // end of test for integer_

STF_CASE_TPL (" shift_right real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using p_t = bs::pack<T>;

  using bs::shift_right;
  using ip_t = bd::as_integer_t<p_t>;
  using r_t = decltype(shift_right(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(shift_right(bs::One<p_t>(),bs::Zero<ip_t>()), bs::One<r_t>());
  STF_EQUAL(shift_right(bs::Zero<p_t>(),bs::One<ip_t>()), bs::Zero<r_t>());
} // end of test for floating_


#ifndef _DEFINITIONS
#define _DEFINITIONS

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/random.hpp>
#include <boost/random.hpp>
#include <stack>
#include <iostream>
#include <vector>

//erdos dependencies
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>

using namespace std;

//==============================================================================
struct node_properties
{
  int component;
};
struct edge_properties
{
};
struct graph_properties
{
  int number_of_components;
};

//--------------------------------------
typedef boost::adjacency_list<
boost::vecS,
boost::vecS,
boost::directedS,
node_properties,
boost::disallow_parallel_edge_tag,
graph_properties> graph_t;

typedef typename boost::graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename boost::graph_traits<graph_t>::edge_descriptor edge_t;

//erdos generator type
typedef boost::erdos_renyi_iterator<boost::minstd_rand, graph_t> ERGen;

//vars used in all agorithms
// vector<bool> visited;
// vector<int> r_index;
// vector<bool> inComponent;
// stack<int> S;
// int vindex;
// int c; // component number.

graph_t *g;

//====utils==================================================================

// //print graph
// void print_graph(const graph_t &graph)
// {
//   cout<<'\n';
//   cout << "Graph ->" << "vertices: "<< boost::num_vertices(graph) << ", edges: " << boost::num_edges(graph) << endl;
//   auto edges = boost::edges(graph);
//   for (auto it = edges.first; it != edges.second; ++it)
//   {
//     cout << boost::source(*it, graph) << " -> "
//     << boost::target(*it, graph) << endl;
//   }
// }

// boost library SCC detection
// used to countercheck the results
void readSCC(graph_t graph_in)
{
  cout<<"Boost Tarjan Start\n";
  int nn = boost::num_vertices(graph_in);
  vector<int> cc(nn);
  int num = strong_components(graph_in, make_iterator_property_map(cc.begin(), get(boost::vertex_index, graph_in), cc[0]));

  // cout << "Boost:: Total number of components: " << num << endl;

 //  for (int comps=0; comps<num; comps++)
 //  {

 //    cout << "Compenent " << comps  << ": ";
 //    for (int i = 0; i < cc.size(); i++)
 //    {
 //      if (comps == cc[i])
 //       cout<<i<<' ';
 //   }
 //   cout<<endl;
 // }
 cout<<"Boost::Tarjan done";
 cout<<endl;
}

#endif


#define BOOST_TEST_MODULE Grid
#include <boost/test/unit_test.hpp>

#include "sycomore/Grid.h"
#include "sycomore/GridScanner.h"
#include "sycomore/magnetization.h"

using Grid = sycomore::Grid<sycomore::ComplexMagnetization>;

BOOST_AUTO_TEST_CASE(EmptyConstructor)
{
    Grid const grid;
    BOOST_TEST(grid.dimension() == 0);
    BOOST_TEST(grid.origin().empty());
    BOOST_TEST(grid.shape().empty());
    BOOST_TEST(grid.stride().empty());
}

BOOST_AUTO_TEST_CASE(UninitializedConstructor)
{
    Grid const grid({-2,-3,-5}, {5,7,11});
    BOOST_TEST(grid.dimension() == 3);
    BOOST_TEST(grid.origin() == sycomore::Index({-2,-3,-5}));
    BOOST_TEST(grid.shape() == sycomore::Shape({5,7,11}));
    BOOST_TEST(grid.stride() == sycomore::Stride({1,5,35,385}));
}

BOOST_AUTO_TEST_CASE(InitializedConstructor)
{
    sycomore::ComplexMagnetization const m{{1, 2}, 3, {4, 5}};
    Grid const grid({-5, -3}, {11, 7}, m);
    BOOST_TEST(grid.dimension() == 2);
    BOOST_TEST(grid.origin() == sycomore::Index({-5, -3}));
    BOOST_TEST(grid.shape() == sycomore::Shape({11,7}));
    BOOST_TEST(grid.stride() == sycomore::Stride({1,11,77}));
    for(int i=0; i<grid.stride()[grid.dimension()]; ++i)
    {
        BOOST_REQUIRE(grid.data()[i] == m);
    }
}

BOOST_AUTO_TEST_CASE(Accessor)
{
    sycomore::ComplexMagnetization const zero{0,0,0};
    sycomore::ComplexMagnetization const non_zero{1,2,3};
    Grid grid({-5, -3}, {11, 7}, zero);
    grid[{-2,-1}] = non_zero;

    auto && grid_const = const_cast<Grid const &>(grid);

    for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
    {
        if(index.first == sycomore::Index{-2,-1})
        {
            BOOST_REQUIRE(grid_const[index.first] == non_zero);
        }
        else
        {
            BOOST_REQUIRE(grid_const[index.first] == zero);
        }
    }
}

BOOST_AUTO_TEST_CASE(ScanOrder)
{
    Grid grid({-5, -3}, {11, 7}, {0, 0, 0});

    unsigned int i=0;
    for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
    {
        grid[index.first] = {
            sycomore::Real(i), sycomore::Real(i), sycomore::Real(i)};
        ++i;
    }

    auto && data = grid.data();
    for(unsigned int i=0; i<grid.stride()[grid.dimension()]; ++i)
    {
        BOOST_REQUIRE(*data == sycomore::ComplexMagnetization({
            sycomore::Real(i), sycomore::Real(i), sycomore::Real(i)}));
        ++data;
    }
}

void compare_grids(
    Grid const & old, Grid const & new_,
    bool is_initialized, Grid::value_type const & value={})
{
    sycomore::GridScanner const generator_old(old.origin(), old.shape());
    std::vector<sycomore::Index> indices_old;
    std::transform(
        generator_old.begin(), generator_old.end(),
        std::back_inserter(indices_old),
        [](sycomore::GridScanner::value_type const & x){ return x.first; });

    sycomore::GridScanner const generator_new(new_.origin(), new_.shape());
    std::vector<sycomore::Index> indices_new;
    std::transform(
        generator_new.begin(), generator_new.end(),
        std::back_inserter(indices_new),
        [](sycomore::GridScanner::value_type const & x){ return x.first; });

    std::vector<sycomore::Index> intersection;
    std::vector<sycomore::Index> difference;
    for(auto && index: indices_new)
    {
        auto && it = std::find(indices_old.begin(), indices_old.end(), index);
        if(it != indices_old.end())
        {
            intersection.push_back(index);
        }
        else
        {
            difference.push_back(index);
        }
    }

    for(auto && index: intersection)
    {
        BOOST_REQUIRE(old[index] == new_[index]);
    }
    if(is_initialized)
    {
        for(auto && index: difference)
        {
            BOOST_REQUIRE(new_[index] == value);
        }
    }
}

BOOST_AUTO_TEST_CASE(ReshapeLargerUninitialized)
{
    Grid old({-2,-1}, {5,3});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({-3,-2}, {7,5});

    BOOST_TEST(new_.origin() == sycomore::Index({-3,-2}));
    BOOST_TEST(new_.shape() == sycomore::Shape({7, 5}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 7, 35}));
    compare_grids(old, new_, false);
}

BOOST_AUTO_TEST_CASE(ReshapeLargerInitialized)
{
    Grid old({-2,-1}, {5,3});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({-3,-2}, {7,5}, {1000, 0, 0});

    BOOST_TEST(new_.origin() == sycomore::Index({-3,-2}));
    BOOST_TEST(new_.shape() == sycomore::Shape({7, 5}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 7, 35}));
    compare_grids(old, new_, true, {1000,0,0});
}

BOOST_AUTO_TEST_CASE(ReshapeSmallerUninitialized)
{
    Grid old({-3,-2}, {7, 5});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({-2,-1}, {5,3});

    BOOST_TEST(new_.origin() == sycomore::Index({-2,-1}));
    BOOST_TEST(new_.shape() == sycomore::Shape({5,3}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 5, 15}));
    compare_grids(old, new_, false);
}

BOOST_AUTO_TEST_CASE(ReshapeSmallerInitialized)
{
    Grid old({-3,-2}, {7, 5});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({-2,-1}, {5,3}, {1000, 0, 0});

    BOOST_TEST(new_.origin() == sycomore::Index({-2,-1}));
    BOOST_TEST(new_.shape() == sycomore::Shape({5,3}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 5, 15}));
    compare_grids(old, new_, true, {1000, 0, 0});
}

BOOST_AUTO_TEST_CASE(ReshapeMixedUninitialized)
{
    Grid old({-3,-2}, {7, 5});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({-5,-1}, {11,3});

    BOOST_TEST(new_.origin() == sycomore::Index({-5,-1}));
    BOOST_TEST(new_.shape() == sycomore::Shape({11,3}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 11, 33}));
    compare_grids(old, new_, false);
}

BOOST_AUTO_TEST_CASE(ReshapeMixedInitialized)
{
    Grid old({-3,-2}, {7, 5});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({-5,-1}, {11,3}, {1000,0,0});

    BOOST_TEST(new_.origin() == sycomore::Index({-5,-1}));
    BOOST_TEST(new_.shape() == sycomore::Shape({11,3}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 11, 33}));
    compare_grids(old, new_, true, {1000,0,0});
}

BOOST_AUTO_TEST_CASE(ReshapeDisjointUninitialized)
{
    Grid old({-3,-2}, {3, 2});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({1,9}, {5,7});

    BOOST_TEST(new_.origin() == sycomore::Index({1,9}));
    BOOST_TEST(new_.shape() == sycomore::Shape({5,7}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 5, 35}));
    compare_grids(old, new_, false);
}

BOOST_AUTO_TEST_CASE(ReshapeDisjointInitialized)
{
    Grid old({-3,-2}, {3, 2});

    int i=1;
    for(auto && index: sycomore::GridScanner(old.origin(), old.shape()))
    {
        old[index.first] = {i,0,0};
        ++i;
    }

    Grid new_(old);
    new_.reshape({1,9}, {5,7}, {1000,0,0});

    BOOST_TEST(new_.origin() == sycomore::Index({1,9}));
    BOOST_TEST(new_.shape() == sycomore::Shape({5,7}));
    BOOST_TEST(new_.stride() == sycomore::Stride({1, 5, 35}));
    compare_grids(old, new_, true, {1000,0,0});
}

BOOST_AUTO_TEST_CASE(ConstIterator)
{
    Grid grid({-3,-2}, {7,5});
    unsigned int i=1;
    for(auto && index: sycomore::GridScanner(grid.origin(), grid.shape()))
    {
        grid[index.first] = {i,0,0};
        ++i;
    }

    Grid const & grid_const = const_cast<Grid const &>(grid);

    i=1;
    for(auto && x: grid_const)
    {
        BOOST_REQUIRE(sycomore::ComplexMagnetization(i,0,0)==x);
        ++i;
    }
}

BOOST_AUTO_TEST_CASE(Iterator)
{
    Grid grid({-3,-2}, {7,5});
    unsigned int i=1;
    for(auto && x: grid)
    {
        x = {i,0,0};
        ++i;
    }

    Grid const & grid_const = const_cast<Grid const &>(grid);

    i=1;
    for(auto && x: grid_const)
    {
        BOOST_REQUIRE(sycomore::ComplexMagnetization(i,0,0)==x);
        ++i;
    }
}


// Library includes
#include <string>
#include <ostream>
#include <iostream>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <thread>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

// Project includes
#include "parseCommandLineOptions.hpp"
#include "hornRegistration.hpp"
#include "PivotCalibration.hpp"
#include "PA3_4_DataConstants.hpp"
#include "parsePA3_4.hpp"
#include "DistortionCalibration.hpp"
#include "IterativeClosestPoint.hpp"

namespace po = boost::program_options;

/// Produce an output CIS CSV file
/// Note: I tried to make an output function but it probably has bugs
template<typename T>
void output1CISCSV_PA3(std::ostream& ostr, const std::string& outputName = "name-output-3.txt", const T & dk = std::vector<Eigen::Vector3d>(), const T & ck = std::vector<Eigen::Vector3d>(), const std::vector<double> & error = std::vector<double>()){

    ostr << dk.size() << " " << outputName << "\n";

    typename T::const_iterator dIterator = dk.begin();
    typename T::const_iterator cIterator = ck.begin();
    std::vector<double>::const_iterator eIterator = error.begin();
    for(; dIterator != dk.end() && cIterator != ck.end() && eIterator != error.end();
        ++dIterator, ++cIterator, ++eIterator){
            ostr  << (*dIterator)(0) << "   "  << (*dIterator)(1) << "  "  << (*dIterator)(2) << "  " << (*cIterator)(0) << "   "  << (*cIterator)(1) << "  "  << (*cIterator)(2) << "  " << *eIterator << "\n";
    }

    ostr.flush();

}

/// read the command line options from argc,argv and load them into the params object
/// @see boost::program_options for details on how the command line parsing library works.
bool readCommandLine(int argc, char* argv[], ParsedCommandLineCommandsPA3_4 & pclp){

    static const bool optional = false; // false means parameters are not required, and therefore optional
    static const bool required = true;  // true means parameters are required

    po::options_description generalOptions("General Options");
    generalOptions.add_options()
    ("responseFile", po::value<std::string>(), "File containing additional command line parameters")
    CLO_HELP
    CLO_DEBUG
    ("debugParser","display debug information for data file parser")
    ;

    TerminationCriteriaParams tcp; // default termination criteria params

    po::options_description algorithmOptions("Algorithm Options");
    algorithmOptions.add_options()
    ("threads","run each source data file in a separate thread. May speed up execution dramatically.")
    ("icpAlgorithm"                         ,po::value<std::string>()->default_value("spatialIndex"         ),"Options: spatialIndex, simpleSearch. Selects the ICP algorithm version to use. spatialIndex provides a substantial performance boost for large data sets. ")
    ("meanErrorThreshold"                   ,po::value<double>()->default_value(tcp.meanErrorThreshold)       ,"stop ICP when mean error drops below this level")
    ("maxErrorThreshold"                    ,po::value<double>()->default_value(tcp.maxErrorThreshold)       ,"stop ICP when max error drops below this level")
    ("minVarianceInMeanErrorBetweenIterations"                   ,po::value<double>()->default_value(tcp.minVarianceInMeanErrorBetweenIterations)       ,"stop ICP when mean error no longer varies between iterations")
    ("minIterationCount"                   ,po::value<int>()->default_value(tcp.minIterationCount)       ,"Do not stop ICP unless this many iterations have run")
    ("maxIterationCount"                    ,po::value<int>()->default_value(tcp.maxIterationCount)       ,"Stop ICP when the maximum iteration count threshold is reached, superceded by minIterationCount")

    ;

    // create algorithm command line options
    //algorithmOptions.add_options()
	// todo, add options for configuring algorithms

    po::options_description dataOptions("Data Options");

    std::string currentPath(boost::filesystem::path( boost::filesystem::current_path() ).string());

    // create algorithm command line options
    dataOptions.add_options()
            ("pa3", "set automatic programming assignment 3 source data parameters, overrides DataFilenamePrefix, exclusive of pa4")
            ("pa4", "set automatic programming assignment 4 source data parameters, overrides DataFilenamePrefix, exclusive of pa3")

            ("dataFolderPath"                   ,po::value<std::string>()->default_value(currentPath)       ,"folder containing data files, defaults to current working directory"   )
            ("outputDataFolderPath"             ,po::value<std::string>()->default_value(currentPath)       ,"folder for output data files, defaults to current working directory"   )
            ("dataFilenamePrefix"               ,po::value<std::vector<std::string> >()->default_value(PA4DataFilePrefixes(),""),"constant prefix of data filename path. Specify this multiple times to run on many data sources at once"   )
			("dataFilenameProblemPrefix"               ,po::value<std::string >()->default_value(pa4problemPrefix),"constant prefix of data typically starting with \"Problem\" filename path. Specify this multiple times to run on many data sources at once"   )
		  	("suffixAnswer"    ,po::value<std::string>()->default_value(DefaultAnswer         ),"suffix of data filename path"   )
		  	("suffixOutput"    ,po::value<std::string>()->default_value(DefaultOutput         ),"suffix of data filename path"   )
		  	("suffixSample"    ,po::value<std::string>()->default_value(DefaultSampleReadings ),"suffix of data filename path"   )
		  	("suffixMesh"      ,po::value<std::string>()->default_value(DefaultMesh           ),"suffix of data filename path"   )
		  	("suffixBodyA"     ,po::value<std::string>()->default_value(DefaultBodyA          ),"suffix of data filename path"   )
		  	("suffixBodyB"     ,po::value<std::string>()->default_value(DefaultBodyB          ),"suffix of data filename path"   )

		  	("AnswerPath"                    ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("OutputPath"                    ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("SamplePath"                    ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("MeshPath"                      ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("BodyAPath"                     ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("BodyBPath"  			         ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("em_fiducialsPath"  			 ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("em_navPath"        			 ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )
		  	("output2Path"       			 ,po::value<std::string>() , "full path to data txt file, optional alternative to prefix+suffix name combination"   )

			  ;

    po::options_description allOptions;
    allOptions.add(generalOptions).add(algorithmOptions).add(dataOptions);

    po::variables_map vmap;

    try
    {
      po::store(po::command_line_parser(argc, argv).options(allOptions).run(), vmap);
      po::notify(vmap);
    }
    catch (std::exception& e)
    {
      std::cerr << "[Error] " << BOOST_CURRENT_FUNCTION << std::endl
      << "    " << e.what() << std::endl
      << std::endl
      << allOptions << std::endl;
      return false;
    }

    if (vmap.count(CLO_GET_ARG_STR2(CLO_HELP)) || argc < 2)
    {
      std::cout << allOptions << std::endl;
      return false;
    }

    pclp.debugParser = vmap.count("debugParser");

	parseResponseFiles(vmap,allOptions);

	// initalize string params
	std::string  dataFolderPath
				,dataFileNameSuffix_AnswerPath
				,dataFileNameSuffix_OutputPath
				,dataFileNameSuffix_SamplePath
				,dataFileNameSuffix_MeshPath
				,dataFileNameSuffix_BodyAPath
				,dataFileNameSuffix_BodyBPath
				;

    DataSourcePA3_4 datasource;

    std::vector<std::string> dataFilenamePrefixList;
	std::string dataFilenameProblemPrefix, icpAlgorithm;

    // load up parameter values from the variable map
    po::readOption(vmap, "dataFolderPath"                  ,dataFolderPath                     ,optional);
    po::readOption(vmap, "outputDataFolderPath"            ,pclp.outputDataFolderPath          ,optional);
	po::readOption(vmap, "dataFilenamePrefix"              ,dataFilenamePrefixList             ,optional);
	po::readOption(vmap, "dataFilenameProblemPrefix"       ,dataFilenameProblemPrefix          ,optional);
	po::readOption(vmap, "suffixAnswer"                    ,dataFileNameSuffix_AnswerPath      ,optional);
	po::readOption(vmap, "suffixOutput"                    ,dataFileNameSuffix_OutputPath      ,optional);
	po::readOption(vmap, "suffixSample"                    ,dataFileNameSuffix_SamplePath      ,optional);
	po::readOption(vmap, "suffixMesh"                      ,dataFileNameSuffix_MeshPath        ,optional);
	po::readOption(vmap, "suffixBodyA"                     ,dataFileNameSuffix_BodyAPath       ,optional);
	po::readOption(vmap, "suffixBodyB"                     ,dataFileNameSuffix_BodyBPath       ,optional);

    /// @todo consider allowing these values to be specified separately for each data source
	po::readOption(vmap, "meanErrorThreshold"              ,tcp.meanErrorThreshold       ,optional);
	po::readOption(vmap, "maxErrorThreshold"               ,tcp.maxErrorThreshold        ,optional);
	po::readOption(vmap, "minVarianceInMeanErrorBetweenIterations"               ,tcp.minVarianceInMeanErrorBetweenIterations        ,optional);
	po::readOption(vmap, "minIterationCount"               ,tcp.minIterationCount        ,optional);
	po::readOption(vmap, "maxIterationCount"               ,tcp.maxIterationCount        ,optional);

    // min iteration count supercedes max
    tcp.maxIterationCount = std::max(tcp.minIterationCount,tcp.maxIterationCount);

    // enable threads if specified
    pclp.threads = vmap.count("threads");

    // enable spatialIndex if selected
	po::readOption(vmap, "icpAlgorithm"                     ,icpAlgorithm       ,optional);
    pclp.useSpatialIndex = (icpAlgorithm == "spatialIndex");

    if (vmap.count("pa3")) {
        dataFilenamePrefixList = PA3DataFilePrefixes();
		dataFilenameProblemPrefix = pa3problemPrefix;
    } else if (vmap.count("pa4")) {
        dataFilenamePrefixList = PA4DataFilePrefixes();
		dataFilenameProblemPrefix = pa4problemPrefix;
    }

    int prefixCount = dataFilenamePrefixList.size();
    if(!prefixCount){
        pclp.dataSources.push_back(DataSourcePA3_4());
    }

    if(prefixCount<=1){
        po::readOption(vmap,"calbodyPath"                      ,pclp.dataSources[0].BodyA            ,optional);
        po::readOption(vmap,"calreadingsPath"                  ,pclp.dataSources[0].BodyB            ,optional);
        po::readOption(vmap,"empivotPath"                      ,pclp.dataSources[0].SampleReadings   ,optional);
        po::readOption(vmap,"optpivotPath"                     ,pclp.dataSources[0].Answer           ,optional);
        po::readOption(vmap,"output1Path"                      ,pclp.dataSources[0].Output           ,optional);
        po::readOption(vmap,"ct_fiducialsPath"                 ,pclp.dataSources[0].Mesh             ,optional);
    }

	// check if the user supplied a full path, if not assemble a path
	// from the default paths and the defualt prefix/suffix combos
    for(auto&& prefix : dataFilenamePrefixList){
        DataSourcePA3_4 dataSource;
		dataSource.terminationCriteriaParams = tcp;
        dataSource.filenamePrefix = prefix;
        dataSource.filenameProblemPrefix = dataFilenameProblemPrefix;
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenameProblemPrefix       ,dataFileNameSuffix_BodyAPath   ,dataSource.BodyA            ,required);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenameProblemPrefix       ,dataFileNameSuffix_BodyBPath   ,dataSource.BodyB            ,required);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenamePrefix              ,dataFileNameSuffix_SamplePath  ,dataSource.SampleReadings   ,required);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenamePrefix              ,dataFileNameSuffix_AnswerPath  ,dataSource.Answer           ,optional);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenamePrefix              ,dataFileNameSuffix_OutputPath  ,dataSource.Output           ,optional);
        assemblePathIfFullPathNotSupplied(dataFolderPath,dataSource.filenameProblemPrefix       ,dataFileNameSuffix_MeshPath    ,dataSource.Mesh             ,required);

        pclp.dataSources.push_back(dataSource);
    }

    pclp.debug = vmap.count(CLO_GET_ARG_STR2(CLO_DEBUG));

    return false;
}

/// run the PA3 algorithm and create then write the output file
void generateOutputFilePA3_4(AlgorithmDataPA3_4 ad, std::string outputDataFolderPath, std::string dataFilenamePrefix, bool useSpatialIndex = false, TerminationCriteriaParams tcp = TerminationCriteriaParams(), bool debug = false){

    Eigen::MatrixXd skMat;
    Eigen::MatrixXd ckMat;

    std::vector<double> errork;
    Eigen::Affine3d Freg;
    TerminationCriteria tc(tcp);
    tc.description = dataFilenamePrefix;

    Eigen::MatrixXd dkMat = dkKnownMeshPointsBaseFrame(ad.sampleReadings.NA, ad.sampleReadings.NB, ad.bodyA.tip, ad.bodyA.markerLEDs, ad.bodyB.markerLEDs);
    if(!useSpatialIndex){ // original slower way
        ICPwithSimpleSearch(dkMat, ad.mesh.vertices, ad.mesh.vertexTriangleNeighborIndex,tc,Freg,skMat,ckMat,errork,debug);
    } else { // new cool fast big data structure way
        ICPwithSpatialIndex(dkMat, ad.mesh.vertices, ad.mesh.vertexTriangleNeighborIndex,tc,Freg,skMat,ckMat,errork,debug);
    }

    std::vector<Eigen::Vector3d> sk(splitVectors(skMat));
    std::vector<Eigen::Vector3d> ck(splitVectors(ckMat));

    std::string outputFilename =  dataFilenamePrefix + "-Output.txt";
    std::ofstream ofs (outputFilename, std::ofstream::out);
    output1CISCSV_PA3(ofs,outputFilename,sk,ck,errork);

    ofs.close();

}

/**************************************************************************/
/**
 * @brief Main function
 *
 * @param argc  Number of input arguments
 * @param argv  Pointer to input arguments
 *
 * @return int
 */
int main(int argc,char**argv) {

	ParsedCommandLineCommandsPA3_4 pclp;
	readCommandLine(argc,argv,pclp);

    // thread pool to speed up execution
    std::vector<std::thread> th;

    for(auto&& dataSource : pclp.dataSources){
        AlgorithmDataPA3_4 ad;
        ad.bodyA          = parseProblemBody    (loadStringFromFile(dataSource.BodyA)            ,pclp.debugParser       );
        ad.bodyB          = parseProblemBody    (loadStringFromFile(dataSource.BodyB)            ,pclp.debugParser       );
		ad.mesh           = parseMesh           (loadStringFromFile(dataSource.Mesh)             ,pclp.debugParser       );
		ad.sampleReadings = parseSampleReadings (loadStringFromFile(dataSource.SampleReadings)   , ad.bodyA.markerLEDs.rows(), ad.bodyB.markerLEDs.rows() ,pclp.debugParser       );

        if(pclp.threads) {
            // run all data sources in separate threads to speed up execution
            th.push_back(std::thread(generateOutputFilePA3_4,ad, pclp.outputDataFolderPath, dataSource.filenamePrefix,pclp.useSpatialIndex,dataSource.terminationCriteriaParams,pclp.debug));
        } else {
            generateOutputFilePA3_4(ad, pclp.outputDataFolderPath, dataSource.filenamePrefix,pclp.useSpatialIndex,dataSource.terminationCriteriaParams,pclp.debug);
        }
    }

    //Join the threads with the main thread
    for(auto &t : th){
        t.join();
    }

	return 0;
}


/*
Copyright (C) 2015-present CompatibL

Performance test results and finance-specific examples are available at:

http://www.tapescript.org

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.
*/

#include <stdlib.h>
#include <iostream>

# define CL_BASE_SERIALIZER_OPEN

#include <cl/tape/tape.hpp>

#include "impl/utils.hpp"

struct test_stream
{
    test_stream(std::ostream& o) : o_(o)
    {}

    template<typename O>
    test_stream & operator << (O const& v)
    {
        using namespace cl;
        o_ << v;
        std::cout << v;
        return *this;
    }

    test_stream& operator<<(std::ostream& (*f)(std::ostream&))
    {
        f(o_);
        f(std::cout);

        return *this;
    }

    std::ostream& o_;
};

#  define out_str test_stream(out_stream)

#include <boost/timer.hpp>

#include "impl/array_examples.hpp"
#include "impl/basic_examples.hpp"
#include "impl/double_examples.hpp"
#include "impl/performance_tests.hpp"
#include "impl/regression_examples.hpp"
#include "impl/quadratic_regression_examples.hpp"
#include "impl/polynomial_regression_examples.hpp"
#include "impl/serialization_example.hpp"

int main()
{
    std::cout << "Run basic_examples see tape data in output/basic_examples_output.txt ..." << std::endl;
    cl::basic_examples();

    std::cout << std::endl << "Run array_examples see tape data in output/array_examples_output.txt ..." << std::endl;
    cl::array_examples();

    std::cout << std::endl << "Run non_optimized_array_examples see tape data in output/non_optimized_array_examples_output.txt ..." << std::endl;
    cl::non_optimized_array_examples();

    std::cout << std::endl << "Run performance_tests see tape data in output/performance_tests_output.txt ..." << std::endl;
    cl::performance_tests();
    /*
    std::cout << std::endl << "Run regression_examples see tape data in output/performance/regression_examples_output.txt ..." << std::endl;
    cl::regression_examples();

    std::cout << std::endl << "Run quadratic_regression_examples see tape data in output/performance/quadratic_regression_examples_output.txt ..." << std::endl;
    cl::quadratic_regression_examples();*/

    std::cout << std::endl << "Run polynomial_regression_examples see tape data in output/performance/polynomial_regression_examples_output.txt ..." << std::endl;
    cl::polynomial_regression_examples();

    system("pause");
    return 0;
}


///////////////////////////////////////////////////////////////////////////////
// A C++ implementation of the Hungarian algorithm for solving the assignment
// problem.
//
// Both this code and the original MATLAB code by Markus Buehren are published
// under the BSD license (see LICENSE.txt).
//
// Copyright (c) 2016, Cong Ma (mcximing)
// Copyright (c) 2018, Alexander Buchegger (abuchegger)
//
#define BOOST_TEST_MODULE hungarian_algorithm

#include "hungarian_algorithm.h"
#include <boost/chrono.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <iostream>

namespace ublas = boost::numeric::ublas;

class GroupingNumPunct : public std::numpunct<char>
{
protected:
  virtual char do_thousands_sep() const
  {
    return ',';
  }

  virtual std::string do_grouping() const
  {
    return "\x03"; // Group every 3 digits
  }
};

struct GroupingNumPunctFixture
{
  GroupingNumPunctFixture()
  {
    std::cout.imbue(std::locale(std::locale::classic(), new GroupingNumPunct));
  }
};

struct DurationFormatter
{
  explicit DurationFormatter(const double d)
    : d_(d)
  {}

  friend std::ostream& operator<<(std::ostream& out, const DurationFormatter& d)
  {
    static const char* const prefixes = " munpfa";
    static const double factors[] = {1e0, 1e3, 1e6, 1e9, 1e12, 1e15, 1e18};
    const char* prefix = &prefixes[0];
    for (const double* factor = &factors[0]; *prefix != 0; ++prefix, ++factor)
    {
      const double value = d.d_ * *factor;
      if (value >= 1.0)
      {
        out << std::fixed << std::setprecision(2) << value;
        if (std::isalpha(*prefix) != 0)
        {
          out << *prefix;
        }
        out << "s";
        return out;
      }
    }
    out << std::scientific << std::setprecision(2) << d.d_ << "s";
    return out;
  }

  const double d_;
};

BOOST_GLOBAL_FIXTURE(GroupingNumPunctFixture);

template<typename T>
void print_matrix(std::ostream& out, const ublas::matrix<T>& matrix, const std::string& indent = std::string(),
                  const std::string& colsep = ", ", const std::string& rowsep = ";\n")
{
  // Save formatting info for later:
  const std::streamsize width = out.width(0);
  const std::streamsize precision = out.precision();
  for (std::size_t row = 0; row < matrix.size1(); ++row)
  {
    out << indent;
    for (std::size_t col = 0; col < matrix.size2(); ++col)
    {
      if (col != 0)
      {
        out << colsep;
      }
      out.width(width);
      out.precision(precision);
      out << matrix(row, col);
    }
    out << rowsep;
  }
}

template<typename Cost>
void print_assignment(std::ostream& out, const std::vector<std::size_t>& assignment,
                      const ublas::matrix<Cost>& cost_matrix, const std::string& indent = std::string())
{
  for (std::size_t row = 0; row < assignment.size(); ++row)
  {
    out << indent << row << " -> ";
    const std::size_t col = assignment[row];
    if (col < cost_matrix.size2())
    {
      out << col << " (cost: " << cost_matrix(row, col) << ")" << std::endl;
    }
    else
    {
      out << "invalid" << std::endl;
    }
  }
}

template<typename T>
ublas::matrix<T> make_matrix(const T* const data, const std::size_t num_rows, const std::size_t num_cols)
{
  ublas::matrix <T> matrix(num_rows, num_cols);
  std::copy(&data[0], &data[num_rows * num_cols], matrix.data().begin());
  return matrix;
}

template<typename T>
std::vector<T> make_vector(const T* const data, const std::size_t size)
{
  return std::vector<T>(&data[0], &data[size]);
}

template<typename SolvingFunction, typename Cost, typename CostComparator>
void assert_solver_result(const SolvingFunction& solving_function, const ublas::matrix<Cost>& cost_matrix,
                          const Cost expected_cost, const std::vector<std::size_t>& expected_assignment,
                          const CostComparator& cost_comparator)
{
  std::vector<std::size_t> assignment;
  const Cost total_cost = solving_function(cost_matrix, cost_matrix.size1(), cost_matrix.size2(), assignment,
                                           cost_comparator);
  BOOST_CHECK_EQUAL(total_cost, expected_cost);
  BOOST_CHECK_EQUAL(assignment.size(), cost_matrix.size1());
  std::size_t num_invalid_assignments = 0;
  for (std::size_t row = 0; row < cost_matrix.size1(); ++row)
  {
    if (assignment.at(row) >= cost_matrix.size2())
    {
      ++num_invalid_assignments;
    }
  }
  const std::size_t expected_num_invalid_assignments = (cost_matrix.size1() > cost_matrix.size2())
                                                       ? cost_matrix.size1() - cost_matrix.size2() : 0;
  BOOST_CHECK_EQUAL(num_invalid_assignments, expected_num_invalid_assignments);
  BOOST_CHECK_EQUAL_COLLECTIONS(assignment.begin(), assignment.end(),
                                expected_assignment.begin(), expected_assignment.end());
}

template<typename Cost, typename CostComparator>
void assert_solvers_result(const ublas::matrix<Cost>& cost_matrix, const Cost expected_cost,
                           const std::vector<std::size_t>& expected_assignment,
                           const CostComparator& cost_comparator)
{
  typedef Cost SolvingFunction(const ublas::matrix<Cost>&, const std::size_t, const std::size_t,
                               std::vector<std::size_t>&, const CostComparator&);
  std::cout << "  Testing Hungarian solver" << std::endl;
  assert_solver_result(static_cast<SolvingFunction*>(&hungarian_algorithm::solve), cost_matrix, expected_cost,
                       expected_assignment, cost_comparator);
  std::cout << "  Testing brute force solver" << std::endl;
  assert_solver_result(static_cast<SolvingFunction*>(&hungarian_algorithm::solveBruteForce), cost_matrix, expected_cost,
                       expected_assignment, cost_comparator);
}

template<typename Cost>
void assert_solvers_result(const ublas::matrix<Cost>& cost_matrix, const Cost expected_cost,
                           const std::vector<std::size_t>& expected_assignment)
{
  assert_solvers_result<Cost>(cost_matrix, expected_cost, expected_assignment, std::less<Cost>());
}

template<typename Cost>
void assert_solvers_return_equal_cost(const ublas::matrix<Cost>& cost_matrix)
{
  std::vector<std::size_t> assignment, assignment_brute_force;
  const Cost total_cost = hungarian_algorithm::solve<Cost>(
    cost_matrix, cost_matrix.size1(), cost_matrix.size2(), assignment);
  const Cost total_cost_brute_force = hungarian_algorithm::solveBruteForce<Cost>(
    cost_matrix, cost_matrix.size1(), cost_matrix.size2(), assignment_brute_force);
  if (total_cost != total_cost_brute_force)
  {
    std::cerr << "total_cost != total_cost_brute_force:" << std::endl;
    std::cerr << "  total_cost: " << total_cost << std::endl;
    std::cerr << "  total_cost_brute_force: " << total_cost_brute_force << std::endl;
    std::cerr << "  cost_matrix: [" << std::endl << std::setw(3);
    print_matrix(std::cerr, cost_matrix, "    ");
    std::cerr << "  ]" << std::endl;
    std::cerr << "  Hungarian assignment:" << std::endl;
    print_assignment(std::cerr, assignment, cost_matrix, "    ");
    std::cerr << "  brute force assignment:" << std::endl;
    print_assignment(std::cerr, assignment_brute_force, cost_matrix, "    ");
  }
  BOOST_CHECK_EQUAL(total_cost, total_cost_brute_force);
}

void fill_matrix_with_random_ints(ublas::matrix<int>& cost_matrix, const int min_cost, const int max_cost)
{
  // Note: to prevent overflows when summing, max_cost should not exceed RAND_MAX / cost_matrix.size1().
  for (std::size_t row = 0; row < cost_matrix.size1(); ++row)
  {
    for (std::size_t col = 0; col < cost_matrix.size2(); ++col)
    {
      cost_matrix(row, col) = min_cost + std::rand() % (max_cost - min_cost);
    }
  }
}

void test_solvers_random_square(const std::size_t num_rows, const std::size_t num_runs, const int min_cost,
                                const int max_cost)
{
  ublas::matrix<int> cost_matrix(num_rows, num_rows, 0);
  for (std::size_t i = 0; i < num_runs; ++i)
  {
    std::cout << ".";
    std::cout.flush();
    fill_matrix_with_random_ints(cost_matrix, min_cost, max_cost);
    assert_solvers_return_equal_cost(cost_matrix);
  }
}

template<typename SolvingFunction, typename Cost>
double measure_solver_timing(const SolvingFunction& solving_function, const ublas::matrix<Cost>& cost_matrix)
{
  typedef boost::chrono::high_resolution_clock Clock;
  std::vector<std::size_t> assignment;
  const Clock::time_point start_time(Clock::now());
  solving_function(cost_matrix, cost_matrix.size1(), cost_matrix.size2(), assignment);
  const boost::chrono::duration<double> dt(Clock::now() - start_time);

  // Compute number of combinations = factorial of larger dimension of matrix:
  double c = 1;  // size_t gets too small fast
  for (std::size_t row = 1; row <= std::max(cost_matrix.size1(), cost_matrix.size2()); ++row)
  {
    c *= row;
  }

  std::cout << "  Time to solve " << cost_matrix.size1() << "x" << cost_matrix.size2() << " assignment problem ("
            << std::resetiosflags(std::ios::floatfield) << std::setprecision(8) << c << " combinations): "
            << DurationFormatter(dt.count()) << " (" << DurationFormatter(dt.count() / c) << " per combination)"
            << std::endl;
  return dt.count();
}

template<typename SolvingFunction>
double test_solver_timing(const SolvingFunction& solving_function, const std::size_t num_rows)
{
  ublas::matrix<int> cost_matrix(num_rows, num_rows, 0);
  fill_matrix_with_random_ints(cost_matrix, 0, 1000);
  return measure_solver_timing(solving_function, cost_matrix);
}

BOOST_AUTO_TEST_CASE(test_matrix)
{
  // Tests whether the matrix class works as we expect it to, so that the solvers and tests work as expected.
  const double costs[] = {10, 19, 8, 15, 0,
                          10, 18, 7, 17, 0,
                          13, 16, 9, 14, 0,
                          12, 19, 8, 18, 0};
  const ublas::matrix<double> cost_matrix(make_matrix(costs, 4, 5));

  BOOST_CHECK_EQUAL(cost_matrix.size1(), 4);
  BOOST_CHECK_EQUAL(cost_matrix.size2(), 5);

  std::size_t i = 0;
  for (std::size_t row = 0; row < cost_matrix.size1(); ++row)
  {
    for (std::size_t col = 0; col < cost_matrix.size2(); ++col)
    {
      BOOST_CHECK_EQUAL(cost_matrix(row, col), costs[i++]);
    }
  }
}

BOOST_AUTO_TEST_CASE(test_solvers_0x0)
{
  const boost::numeric::ublas::matrix<int> cost_matrix(0, 0, 0);
  const std::vector<std::size_t> expected_assignment;
  assert_solvers_result(cost_matrix, 0, expected_assignment);
}

BOOST_AUTO_TEST_CASE(test_solvers_0x2)
{
  const boost::numeric::ublas::matrix<int> cost_matrix(0, 2, 0);
  const std::vector<std::size_t> expected_assignment;
  assert_solvers_result(cost_matrix, 0, expected_assignment);
}

BOOST_AUTO_TEST_CASE(test_solvers_2x0)
{
  const boost::numeric::ublas::matrix<int> cost_matrix(2, 0, 0);
  const std::vector<std::size_t> expected_assignment(2, 0);
  assert_solvers_result(cost_matrix, 0, expected_assignment);
}

BOOST_AUTO_TEST_CASE(test_solvers_1x1)
{
  const boost::numeric::ublas::matrix<int> cost_matrix(1, 1, 1);
  const std::vector<std::size_t> expected_assignment(1, 0);
  assert_solvers_result(cost_matrix, 1, expected_assignment);
}

BOOST_AUTO_TEST_CASE(test_solvers_1x2)
{
  const int costs[] = {1, 0};
  ublas::matrix<int> cost_matrix(1, 2);
  std::copy(&costs[0], &costs[2], cost_matrix.data().begin());
  const std::vector<std::size_t> expected_assignment(1, 1);
  assert_solvers_result(cost_matrix, 0, expected_assignment);
}

BOOST_AUTO_TEST_CASE(test_solvers_2x1)
{
  const int costs[] = {1,
                       0};
  const std::size_t expected_assignment[] = {1, 0};
  assert_solvers_result(make_matrix(costs, 2, 1), 0, make_vector(expected_assignment, 2));
}

BOOST_AUTO_TEST_CASE(test_solvers_2x2)
{
  const int costs[] = {2, 4,
                       0, 1};
  const std::size_t expected_assignment[] = {0, 1};
  assert_solvers_result(make_matrix(costs, 2, 2), 3, make_vector(expected_assignment, 2));
}

BOOST_AUTO_TEST_CASE(test_solvers_2x3)
{
  const int costs[] = {1, 2, 0,
                       1, 3, 0};
  const std::size_t expected_assignment[] = {0, 2};
  assert_solvers_result(make_matrix(costs, 2, 3), 1, make_vector(expected_assignment, 2));
}

BOOST_AUTO_TEST_CASE(test_solvers_3x2)
{
  const int costs[] = {1, 2,
                       0, 1,
                       3, 0};
  const std::size_t expected_assignment[] = {2, 0, 1};
  assert_solvers_result(make_matrix(costs, 3, 2), 0, make_vector(expected_assignment, 3));
}

BOOST_AUTO_TEST_CASE(test_solvers_3x3)
{
  const int costs[] = {1, 2, 4,
                       0, 2, -1,
                       3, 0, -3};
  const std::size_t expected_assignment[] = {1, 0, 2};
  assert_solvers_result(make_matrix(costs, 3, 3), -1, make_vector(expected_assignment, 3));
}

BOOST_AUTO_TEST_CASE(test_solvers_3x3_nonnumeric)
{
  using boost::chrono::seconds;
  const seconds costs[] = {seconds(1), seconds(2), seconds(4),
                           seconds(0), seconds(2), seconds(-1),
                           seconds(3), seconds(0), seconds(-3)};
  const std::size_t expected_assignment[] = {1, 0, 2};
  assert_solvers_result(make_matrix(costs, 3, 3), seconds(-1), make_vector(expected_assignment, 3));
}

BOOST_AUTO_TEST_CASE(test_solvers_3x3_nonnumeric_maximize)
{
  using boost::chrono::seconds;
  const seconds costs[] = {seconds(1), seconds(2), seconds(4),
                           seconds(0), seconds(2), seconds(-1),
                           seconds(3), seconds(0), seconds(-3)};
  const std::size_t expected_assignment[] = {2, 1, 0};
  assert_solvers_result(make_matrix(costs, 3, 3), seconds(9), make_vector(expected_assignment, 3),
                        std::greater<seconds>());
}

BOOST_AUTO_TEST_CASE(test_solvers_4x5)
{
  const double costs[] = {10, 19, 8, 15, 0,
                          10, 18, 7, 17, 0,
                          13, 16, 9, 14, 0,
                          12, 19, 8, 18, 0};
  const std::size_t expected_assignment[] = {0, 2, 3, 4};
  assert_solvers_result(make_matrix(costs, 4, 5), 31.0, make_vector(expected_assignment, 4));
}

BOOST_AUTO_TEST_CASE(test_solvers_random_nonnegative_int_squares)
{
  std::cout << "  ";
  test_solvers_random_square(8, 10, 0, 1000);
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(test_solvers_random_int_squares)
{
  std::cout << "  ";
  test_solvers_random_square(8, 10, -1000, 1000);
  std::cout << std::endl;
}

BOOST_AUTO_TEST_CASE(test_solvers_timings)
{
  // These are informative only, and to check for any severe errors (segfaults, endless loops, ...):
  typedef int SolvingFunction(const ublas::matrix<int>&, const std::size_t, const std::size_t,
                              std::vector<std::size_t>&);
  std::cout << "Brute force method:" << std::endl;
  const double brute_force_timing = test_solver_timing(
    static_cast<SolvingFunction*>(&hungarian_algorithm::solveBruteForce), 10);
  std::cout << "Hungarian method:" << std::endl;
  const double hungarian_timing = test_solver_timing(static_cast<SolvingFunction*>(&hungarian_algorithm::solve), 10);
  BOOST_CHECK_LT(hungarian_timing, brute_force_timing);
  test_solver_timing(static_cast<SolvingFunction*>(&hungarian_algorithm::solve), 100);
  test_solver_timing(static_cast<SolvingFunction*>(&hungarian_algorithm::solve), 200);
}


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

#include <boost/test/unit_test.hpp>

#include <limbo/opt/adam.hpp>
#include <limbo/opt/chained.hpp>
#include <limbo/opt/cmaes.hpp>
#include <limbo/opt/gradient_ascent.hpp>
#include <limbo/opt/grid_search.hpp>
#include <limbo/opt/parallel_repeater.hpp>
#include <limbo/opt/random_point.hpp>
#include <limbo/opt/rprop.hpp>
#include <limbo/tools/macros.hpp>

using namespace limbo;

struct Params {
    struct opt_gridsearch {
        BO_PARAM(int, bins, 20);
    };

    struct opt_parallelrepeater {
        BO_PARAM(int, repeats, 2);
        BO_PARAM(double, epsilon, 0.1);
    };

    struct opt_rprop : public defaults::opt_rprop {
        BO_PARAM(int, iterations, 150);
    };

    struct opt_gradient_ascent : public defaults::opt_gradient_ascent {
        BO_PARAM(int, iterations, 150);
        BO_PARAM(double, alpha, 0.1);
    };

    struct opt_adam : public defaults::opt_adam {
        BO_PARAM(int, iterations, 150);
        BO_PARAM(double, alpha, 0.1);
    };
};

// test with a standard function
int monodim_calls = 0;
opt::eval_t acqui_mono(const Eigen::VectorXd& v, bool eval_grad)
{
    assert(!eval_grad);
    monodim_calls++;
    return opt::no_grad(3 * v(0) + 5);
}

// test with a functor
int bidim_calls = 0;
struct FakeAcquiBi {
    opt::eval_t operator()(const Eigen::VectorXd& v, bool eval_grad) const
    {
        assert(!eval_grad);
        bidim_calls++;
        return opt::no_grad(3 * v(0) + 5 - 2 * v(1) - 5 * v(1) + 2);
    }
};

// test with gradient
int simple_calls = 0;
bool check_grad = false;
std::vector<Eigen::VectorXd> starting_points;
opt::eval_t simple_func(const Eigen::VectorXd& v, bool eval_grad)
{
    assert(!check_grad || eval_grad);
    simple_calls++;
    starting_points.push_back(v);
    return {-(v(0) * v(0) + 2. * v(0)), limbo::tools::make_vector(-(2 * v(0) + 2.))};
}

BOOST_AUTO_TEST_CASE(test_random_mono_dim)
{
    using namespace limbo;

    opt::RandomPoint<Params> optimizer;

    monodim_calls = 0;
    for (int i = 0; i < 1000; i++) {
        Eigen::VectorXd best_point = optimizer(acqui_mono, Eigen::VectorXd::Constant(1, 0.5), true);
        BOOST_CHECK_EQUAL(best_point.size(), 1);
        BOOST_CHECK(best_point(0) > 0 || std::abs(best_point(0)) < 1e-7);
        BOOST_CHECK(best_point(0) < 1 || std::abs(best_point(0) - 1) < 1e-7);
    }
}

BOOST_AUTO_TEST_CASE(test_random_bi_dim)
{
    using namespace limbo;

    opt::RandomPoint<Params> optimizer;

    bidim_calls = 0;
    for (int i = 0; i < 1000; i++) {
        Eigen::VectorXd best_point = optimizer(FakeAcquiBi(), Eigen::VectorXd::Constant(2, 0.5), true);
        BOOST_CHECK_EQUAL(best_point.size(), 2);
        BOOST_CHECK(best_point(0) > 0 || std::abs(best_point(0)) < 1e-7);
        BOOST_CHECK(best_point(0) < 1 || std::abs(best_point(0) - 1) < 1e-7);
        BOOST_CHECK(best_point(1) > 0 || std::abs(best_point(1)) < 1e-7);
        BOOST_CHECK(best_point(1) < 1 || std::abs(best_point(1) - 1) < 1e-7);
    }
}

BOOST_AUTO_TEST_CASE(test_grid_search_mono_dim)
{
    using namespace limbo;

    opt::GridSearch<Params> optimizer;

    monodim_calls = 0;
    Eigen::VectorXd best_point = optimizer(acqui_mono, Eigen::VectorXd::Constant(1, 0.5), true);

    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK_CLOSE(best_point(0), 1, 0.0001);
    BOOST_CHECK_EQUAL(monodim_calls, Params::opt_gridsearch::bins() + 1);
}

BOOST_AUTO_TEST_CASE(test_grid_search_bi_dim)
{
    using namespace limbo;

    opt::GridSearch<Params> optimizer;

    bidim_calls = 0;
    Eigen::VectorXd best_point = optimizer(FakeAcquiBi(), Eigen::VectorXd::Constant(2, 0.5), true);

    BOOST_CHECK_EQUAL(best_point.size(), 2);
    BOOST_CHECK_CLOSE(best_point(0), 1, 0.0001);
    BOOST_CHECK_SMALL(best_point(1), 0.000001);
    // TO-DO: Maybe alter a little grid search so not to call more times the utility function
    BOOST_CHECK_EQUAL(bidim_calls, (Params::opt_gridsearch::bins() + 1) * (Params::opt_gridsearch::bins() + 1) + 21);
}

BOOST_AUTO_TEST_CASE(test_gradient)
{
    using namespace limbo;

    opt::Rprop<Params> optimizer;

    simple_calls = 0;
    check_grad = true;
    Eigen::VectorXd best_point = optimizer(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, Params::opt_rprop::iterations());
}

BOOST_AUTO_TEST_CASE(test_classic_optimizers)
{
    using namespace limbo;

    struct MomentumParams {
        struct opt_gradient_ascent : public defaults::opt_gradient_ascent {
            BO_PARAM(int, iterations, 150);
            BO_PARAM(double, alpha, 0.1);
            BO_PARAM(double, gamma, 0.8);
        };
    };

    struct NesterovParams {
        struct opt_gradient_ascent : public defaults::opt_gradient_ascent {
            BO_PARAM(int, iterations, 150);
            BO_PARAM(double, alpha, 0.1);
            BO_PARAM(double, gamma, 0.8);
            BO_PARAM(bool, nesterov, true);
        };
    };

    opt::Rprop<Params> rprop;
    opt::Adam<Params> adam;
    opt::GradientAscent<Params> gradient_ascent;
    opt::GradientAscent<MomentumParams> gradient_ascent_momentum;
    opt::GradientAscent<NesterovParams> gradient_ascent_nesterov;

    simple_calls = 0;
    check_grad = true;
    Eigen::VectorXd best_point = rprop(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, Params::opt_rprop::iterations());

    double best_rprop = best_point(0);

    simple_calls = 0;
    check_grad = true;
    best_point = gradient_ascent(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, Params::opt_gradient_ascent::iterations());

    double best_gradient_ascent = best_point(0);

    simple_calls = 0;
    check_grad = true;
    best_point = gradient_ascent_momentum(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, MomentumParams::opt_gradient_ascent::iterations());

    double best_gradient_ascent_momentum = best_point(0);

    simple_calls = 0;
    check_grad = true;
    best_point = gradient_ascent_nesterov(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, NesterovParams::opt_gradient_ascent::iterations());

    double best_gradient_ascent_nesterov = best_point(0);

    simple_calls = 0;
    check_grad = true;
    best_point = adam(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, Params::opt_adam::iterations());

    double best_adam = best_point(0);

    BOOST_CHECK(std::abs(best_rprop - best_gradient_ascent) < 1e-3);
    BOOST_CHECK(std::abs(best_rprop - best_gradient_ascent_momentum) < 1e-3);
    BOOST_CHECK(std::abs(best_rprop - best_gradient_ascent_nesterov) < 1e-3);
    BOOST_CHECK(std::abs(best_rprop - best_adam) < 1e-3);
}

BOOST_AUTO_TEST_CASE(test_parallel_repeater)
{
#ifdef USE_TBB
    static tbb::task_scheduler_init init(1);
#endif
    using namespace limbo;

    opt::ParallelRepeater<Params, opt::Rprop<Params>> optimizer;

    simple_calls = 0;
    check_grad = false;
    starting_points.clear();
    Eigen::VectorXd best_point = optimizer(simple_func, Eigen::VectorXd::Constant(1, 2.0), false);
    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(std::abs(best_point(0) + 1.) < 1e-3);
    BOOST_CHECK_EQUAL(simple_calls, Params::opt_parallelrepeater::repeats() * Params::opt_rprop::iterations() + Params::opt_parallelrepeater::repeats());
    BOOST_CHECK_EQUAL(starting_points.size(), simple_calls);
    BOOST_CHECK(starting_points[0](0) >= 2. - Params::opt_parallelrepeater::epsilon() && starting_points[0](0) <= 2. + Params::opt_parallelrepeater::epsilon());
    BOOST_CHECK(starting_points[Params::opt_rprop::iterations() + 1](0) >= 2. - Params::opt_parallelrepeater::epsilon() && starting_points[Params::opt_rprop::iterations() + 1](0) <= 2. + Params::opt_parallelrepeater::epsilon());
#ifdef USE_TBB
    tools::par::init();
#endif
}

BOOST_AUTO_TEST_CASE(test_chained)
{
    using namespace limbo;

    using opt_1_t = opt::GridSearch<Params>;
    using opt_2_t = opt::RandomPoint<Params>;
    using opt_3_t = opt::GridSearch<Params>;
    using opt_4_t = opt::GridSearch<Params>;
    opt::Chained<Params, opt_1_t, opt_2_t, opt_3_t, opt_4_t> optimizer;

    monodim_calls = 0;
    Eigen::VectorXd best_point = optimizer(acqui_mono, Eigen::VectorXd::Constant(1, 0.5), true);

    BOOST_CHECK_EQUAL(best_point.size(), 1);
    BOOST_CHECK(best_point(0) > 0 || std::abs(best_point(0)) < 1e-7);
    BOOST_CHECK(best_point(0) < 1 || std::abs(best_point(0) - 1) < 1e-7);
    BOOST_CHECK_EQUAL(monodim_calls, (Params::opt_gridsearch::bins() + 1) * 3);
}