// Copyright 2021 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

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

#ifndef STATE_ESTIMATION_NODES__HISTORY_HPP_
#define STATE_ESTIMATION_NODES__HISTORY_HPP_

#include <common/types.hpp>
#include <helper_functions/mahalanobis_distance.hpp>
#include <mpark_variant_vendor/variant.hpp>
#include <state_estimation_nodes/steady_time_grid.hpp>

#include <Eigen/Cholesky>
#include <Eigen/Core>

#include <chrono>
#include <map>
#include <memory>
#include <utility>

namespace autoware
{
namespace common
{
namespace state_estimation
{

namespace detail
{

///
/// Check if the sample passes the Mahalanobis gate.
///
/// @param[in]  sample                     The input sample
/// @param[in]  mean                       The mean to be compared against
/// @param[in]  covariance                 Covariance around the given mean
/// @param[in]  mahalanobis_threshold      The mahalanobis threshold factor
///
/// @tparam     kNumOfStates               Number of states in the state vector.
///
/// @return     True if the sample passes the gate and false otherwise.
///
template<std::int32_t kNumOfStates>
bool passes_mahalanobis_gate(
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, 1> & sample,
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, 1> & mean,
  const Eigen::Matrix<common::types::float32_t, kNumOfStates, kNumOfStates> & covariance,
  const common::types::float32_t mahalanobis_threshold)
{
  using Matrix = Eigen::Matrix<common::types::float32_t, kNumOfStates, kNumOfStates>;
  const Matrix L = covariance.llt().matrixL();
  const auto squared_mahalanobis_distance =
    autoware::common::helper_functions::calculate_squared_mahalanobis_distance(sample, mean, L);
  const auto squared_threshold = mahalanobis_threshold * mahalanobis_threshold;
  return squared_mahalanobis_distance < squared_threshold;
}

}  // namespace detail

/// @brief      An event to indicate a prediction step.
struct PredictionEvent {};

///
/// @brief      An event to reset the state of the filter.
///
/// @tparam     FilterT  Type of the EKF filter used to infer vector and matrix types.
///
template<typename FilterT>
struct ResetEvent
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typename FilterT::State state;
  typename FilterT::State::Matrix covariance;
};

///
/// @brief      This class encapsulates a history of events used with EKF.
///
///             The class handles adding events to a history of a specified size. It models the
///             behavior of a circular buffer, meaning that as new events come in, the oldest ones
///             are removed. The events can be either measurement types or specific events like
///             reset or prediction. Whenever an event is added to the middle of the history all the
///             following events get rolled on top of this event to produce a new state.
///
/// @tparam     FilterT       Type of EKF filter used.
/// @tparam     kNumOfStates  Dimensionality of the state in the filter.
/// @tparam     EventT        A variadic template of all possible events.
///
template<typename FilterT, typename ... EventT>
class History
{
  ///
  /// @brief      A single entry in the history. Holds state and covariance and a variant of events.
  ///
  class HistoryEntry;
  ///
  /// @brief      A functor that handles updating the state. It is used with the variant stored
  ///             within the HistoryEntry.
  ///
  class EkfStateUpdater;

  /// Typedef for timestamps.
  using Timestamp = std::chrono::system_clock::time_point;
  /// Typedef for history map type.
  using HistoryMap = std::multimap<Timestamp, HistoryEntry>;

public:
  ///
  /// @brief      Construct history from a filter pointer with a specific size.
  ///
  /// @param      filter                 The filter pointer to be used internally.
  /// @param[in]  max_history_size       The maximum history size.
  /// @param[in]  mahalanobis_threshold  The mahalanobis threshold
  ///
  explicit History(
    FilterT & filter,
    const std::size_t max_history_size,
    const common::types::float32_t mahalanobis_threshold)
  : m_filter{filter},
    m_max_history_size{max_history_size},
    m_mahalanobis_threshold{mahalanobis_threshold} {}

  ///
  /// @brief      Add an event to history. If it is added to the middle the following ones are
  ///             automatically replayed on top of it.
  ///
  /// @param[in]  timestamp  The timestamp of the event.
  /// @param[in]  entry      The entry to be added to history.
  ///
  void emplace_event(const Timestamp & timestamp, const HistoryEntry & entry);
  /// @brief      Check if the history is empty.
  inline bool empty() const noexcept {return m_history.empty();}
  /// @brief      Get size of history.
  inline std::size_t size() const noexcept {return m_history.size();}
  /// @brief      Get last timestamp in history.
  inline const Timestamp & get_last_timestamp() const noexcept {return m_history.rbegin()->first;}
  /// @brief      Get last event in history.
  inline const HistoryEntry & get_last_event() const noexcept {return m_history.rbegin()->second;}
  /// @brief      Get the filter as a const ref.
  const FilterT & get_filter() const noexcept {return m_filter;}
  /// @brief      Get the filter.
  FilterT & get_filter() noexcept {return m_filter;}

private:
  ///
  /// @brief      If the history is too large, drop the oldest event from it.
  ///
  inline void drop_oldest_event_if_needed()
  {
    if ((m_history.size() >= m_max_history_size) && (m_max_history_size > 0U)) {
      (void) m_history.erase(m_history.begin());
    }
  }

  ///
  /// @brief      Update all the following events as their state is based on the current one.
  ///
  /// @param[in]  start_iter  The current iterator with the new state.
  ///
  void update_impacted_events(const typename HistoryMap::iterator & start_iter);

  HistoryMap m_history{};  ///< history of events.
  FilterT & m_filter{};  ///< pointer to the filter implementation.
  std::size_t m_max_history_size{};  ///< Maximum number of events in history.
  common::types::float32_t m_mahalanobis_threshold{};  ///< Mahalanobis distance threshold.
};

template<typename FilterT, typename ... EventT>
class History<FilterT, EventT...>::HistoryEntry
{
public:
  // cppcheck-suppress unknownMacro  // cppcheck seems to be confused due to lots of templates.
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  template<typename SingleEventT>
  // cppcheck-suppress noExplicitConstructor; Conversion to the variant type takes place.
  HistoryEntry(const SingleEventT & event) : m_event {event} {}

  /// @brief      Update the stored state.
  void update_stored_state(const typename FilterT::State & state) noexcept
  {
    m_stored_state = state;
  }
  /// @brief      Get the stored state.
  const typename FilterT::State & stored_state() const noexcept {return m_stored_state;}
  /// @brief      Set the stored covariance.
  void update_stored_covariance(const typename FilterT::State::Matrix & covariance) noexcept
  {
    m_stored_covariance = covariance;
  }
  /// @brief      Get the stored covariance.
  const typename FilterT::State::Matrix & stored_covariance() const noexcept
  {
    return m_stored_covariance;
  }
  /// @brief      Get the event stored in this history entry.
  const mpark::variant<EventT...> & event() const {return m_event;}

private:
  /// State stored in this entry.
  typename FilterT::State m_stored_state{};
  /// Covariance stored in this entry.
  typename FilterT::State::Matrix m_stored_covariance{FilterT::State::Matrix::Zero()};
  /// Event stored in this history entry.
  mpark::variant<EventT...> m_event;
};

template<typename FilterT, typename ... EventT>
class History<FilterT, EventT...>::EkfStateUpdater
{
public:
  explicit EkfStateUpdater(
    FilterT & filter,
    const common::types::float32_t mahalanobis_threshold,
    const std::chrono::system_clock::duration & dt = std::chrono::milliseconds{0})
    : m_filter {filter}, m_mahalanobis_threshold{mahalanobis_threshold}, m_dt{dt}
  {}

  ///
  /// @brief      An operator that passes a measurement event to the filter implementation.
  ///
  /// @param[in]  event         A measurement event.
  ///
  /// @tparam     MeasurementT  Type of measurement event. Can be any measurement type.
  ///
  template<typename MeasurementT>
  void operator()(const MeasurementT & event)
  {
    m_filter.predict(m_dt);
    // TODO(#887): 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.
    const auto measurement_as_state = event.map_into(m_filter.state());
    if (!detail::passes_mahalanobis_gate(
        measurement_as_state.vector(),
        m_filter.state().vector(),
        m_filter.covariance(),
        m_mahalanobis_threshold)) {return;}
    m_filter.correct(event);
  }

  /// @brief      An operator that resets the state of the filter implementation.
  void operator()(const ResetEvent<FilterT> & event)
  {
    m_filter.reset(event.state, event.covariance);
  }

  /// @brief      An operator that applies the prediction event to the filter implementation.
  void operator()(const PredictionEvent &)
  {
    m_filter.predict(m_dt);
  }

private:
  FilterT & m_filter{};  ///< A pointer to the filter implementation.
  common::types::float32_t m_mahalanobis_threshold{};  ///< Mahalanobis distance threshold.
  std::chrono::system_clock::duration m_dt{};  ///< Current time step.
};

template<typename FilterT, typename ... EventT>
void History<FilterT, EventT...>::emplace_event(
  const Timestamp & timestamp, const HistoryEntry & entry)
{
  drop_oldest_event_if_needed();
  const auto iterator_to_inserted_position = m_history.emplace(timestamp, entry);
  update_impacted_events(iterator_to_inserted_position);
}

template<typename FilterT, typename ... EventT>
void History<FilterT, EventT...>::update_impacted_events(
  const typename HistoryMap::iterator & start_iter)
{
  Timestamp previous_timestamp{};
  if (start_iter == m_history.begin()) {
    if (!mpark::holds_alternative<ResetEvent<FilterT>>(start_iter->second.event())) {
      (void) m_history.erase(start_iter);
      throw std::runtime_error(
              "Non-reset event inserted to the beginning of history. This might "
              "happen if a very old event is inserted into the queue. Consider "
              "increasing the queue size or debug program latencies.");
    }
  } else {
    const auto prev_iter = std::prev(start_iter);
    previous_timestamp = prev_iter->first;
    const auto & prev_entry = prev_iter->second;
    m_filter.reset(
      typename FilterT::State{prev_entry.stored_state()},
      prev_entry.stored_covariance());
  }
  for (auto iter = start_iter; iter != m_history.end(); ++iter) {
    const auto current_timestamp = iter->first;
    auto & entry = iter->second;
    mpark::visit(
      EkfStateUpdater{m_filter, m_mahalanobis_threshold, current_timestamp - previous_timestamp},
      entry.event());
    entry.update_stored_state(m_filter.state());
    entry.update_stored_covariance(m_filter.covariance());
    previous_timestamp = iter->first;
  }
}

}  // namespace state_estimation
}  // namespace common
}  // namespace autoware

#endif  // STATE_ESTIMATION_NODES__HISTORY_HPP_


#include "step_crawling.h"

#include <fstream>
#include <cstdlib>  // For srand() and rand()

#include <Eigen/SVD>
#include <RobotUtilities/utilities.h>
#include <RobotUtilities/TimerLinux.h>

#include <solvehfvc.h>

#define PI 3.14159265

using namespace RUT;
using std::cout;
using std::cerr;
using std::endl;
using std::string;
using Eigen::Vector2d;
using Eigen::Matrix2d;

bool StepCrawlingTaskServer::initStepCrawlingTaskServer() {
  ROS_INFO_STREAM("Step crawling server is starting");
  if (_ros_handle_p == nullptr) {
    ROS_ERROR_STREAM("[StepCrawlingTaskServer] You must call .init() before .initStepCrawlingTaskServer().");
    exit(1);
  }

  _ros_handle_p->param(string("/task/v_singular_value_threshold"),
      _v_singular_value_threshold, 0.1);
  _ros_handle_p->param(string("/task/f_singular_value_threshold"),
      _f_singular_value_threshold, 0.1);
  if (!_ros_handle_p->hasParam("/task/v_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/v_singular_value_threshold] not found");
  if (!_ros_handle_p->hasParam("/task/f_singular_value_threshold"))
      ROS_WARN_STREAM("Parameter [/task/f_singular_value_threshold] not found");

  _ros_handle_p->param(std::string("/task/time_step"), _kTimeStepSec, 0.1);
  if (!_ros_handle_p->hasParam("/task/time_step"))
    ROS_WARN_STREAM("Parameter [/task/time_step] not found");
  _ros_handle_p->param(std::string("/task/number_of_time_steps"), _kNumOfTimeSteps, 30);
  if (!_ros_handle_p->hasParam("/task/number_of_time_steps"))
    ROS_WARN_STREAM("Parameter [/task/number_of_time_steps] not found");
  _ros_handle_p->param(std::string("/task/goal_veloicty_meter"), _kGoalVelocityM, 0.01);
  if (!_ros_handle_p->hasParam("/task/goal_veloicty_meter"))
    ROS_WARN_STREAM("Parameter [/task/goal_veloicty_meter] not found");
  _ros_handle_p->param(std::string("/task/engage_step_length"), _kEngageStepLength, 0.001);
  if (!_ros_handle_p->hasParam("/task/engage_step_length"))
    ROS_WARN_STREAM("Parameter [/task/engage_step_length] not found");
  _ros_handle_p->param(std::string("/task/min_normal_force"), _kNormalForceMin, 4.0);
  if (!_ros_handle_p->hasParam("/task/min_normal_force"))
    ROS_WARN_STREAM("Parameter [/task/min_normal_force] not found");

  _F_W(0) = 0;
  _F_W(1) = 1;
  _F_W(2) = 0;

  return true;
}

bool StepCrawlingTaskServer::hostServices() {
  // --------------------------------------------------------
  // Establish Services
  // --------------------------------------------------------
  ros::ServiceServer reset_service            = _ros_handle_p->advertiseService("reset", &StepCrawlingTaskServer::SrvReset, (RobotBridge*)this);
  ros::ServiceServer move_tool_service        = _ros_handle_p->advertiseService("move_tool", &StepCrawlingTaskServer::SrvMoveTool, (RobotBridge*)this);
  ros::ServiceServer move_until_touch_service = _ros_handle_p->advertiseService("move_until_touch", &StepCrawlingTaskServer::SrvMoveUntilTouch, (RobotBridge*)this);
  ros::ServiceServer get_pose_service         = _ros_handle_p->advertiseService("get_pose", &StepCrawlingTaskServer::SrvGetPose, (RobotBridge*)this);
  ros::ServiceServer execute_task_service     = _ros_handle_p->advertiseService("execute_task", &StepCrawlingTaskServer::SrvExecuteTask, this);

  cout << endl << "[StepCrawlingTaskServer] Service servers are listening.." << endl;
  ros::spin();

  ROS_INFO_STREAM(endl << "[StepCrawlingTaskServer] Service servers stopped." << endl);
  return true;
}

bool StepCrawlingTaskServer::SrvExecuteTask(std_srvs::Empty::Request  &req,
    std_srvs::Empty::Response &res) {
  _controller.reset();

  if (_controller._f_queue.size() < 50) {
    cout << "Run update() for " << _main_loop_rate << " frames:" << endl;
    // first, run update for 1s to populate the data deques
    ros::Rate pub_rate(_main_loop_rate);
    for (int i = 0; i < _main_loop_rate; ++i) {
      bool b_is_safe = _controller.update();
      // if(!b_is_safe) break;
      pub_rate.sleep();
    }
    cout << "Done." << endl;
  }

  Timer timer;
  std::srand(std::time(0));
  MatrixXd f_data, v_data;
  for (int fr = 0; fr < _kNumOfTimeSteps; ++fr) {
    timer.tic();
    /**
     * Estimate Natural Constraints
     */
    // get the weighted data
    f_data = MatrixXd::Zero(3, _controller._f_queue.size());
    v_data = MatrixXd::Zero(3, _controller._v_queue.size());
    for (int i = 0; i < _controller._f_queue.size(); ++i)
      f_data.col(i) = _controller._f_queue[i].head(3) * _controller._f_weights[i];
    for (int i = 0; i < _controller._v_queue.size(); ++i)
      v_data.col(i) = _controller._v_queue[i].head(3) * _controller._v_weights[i];

    // SVD on velocity data
    Eigen::JacobiSVD<MatrixXd> svd_v(v_data.transpose(), Eigen::ComputeThinV);
    VectorXd sigma_v = svd_v.singularValues();
    int DimV = 0;
    for (int i = 0; i < 3; ++i)
      if (sigma_v(i) > _v_singular_value_threshold) DimV ++;

    // get a basis for row space of velocity data
    MatrixXd rowspace_v = svd_v.matrixV().leftCols(DimV);

    // filter out force data that:
    //    1. has a small weight
    std::vector<int> f_id;
    for (int i = 0; i < f_data.cols(); ++i) {
      double length = f_data.col(i).norm();
      if (length > 1.5) { // weighted length in newton
        f_id.push_back(i);
      }
    }

    double pose_fb[7], pose_set[7];
    _robot.getPose(pose_fb);
    Matrix3d R_WT = quat2SO3(pose_fb[3], pose_fb[4], pose_fb[5], pose_fb[6]);

    int f_data_length = f_id.size();
    if (f_data_length < 5) {
      ROS_WARN_STREAM("[StepCrawlingTaskServer] lacks significant force data");
      // stabilize onto the surface.
      Vector3d F_T = R_WT.transpose() * _F_W.normalized() * _kNormalForceMin;
      double force_temp[6] = {- F_T(0), - F_T(1), - F_T(2), 0, 0, 0};
      _controller.ExecuteHFVC(6, 0, Matrix6d::Identity(), pose_fb, force_temp,
          HS_CONTINUOUS, _main_loop_rate, 10*_kTimeStepSec);
      continue;
    }
    int DimF = 1;

    MatrixXd f_data_filtered = MatrixXd::Zero(3, f_data_length);
    for (int i = 0; i < f_data_length; ++i)
      f_data_filtered.col(i) = f_data.col(f_id[i]);

    MatrixXd f_data_selected;
    // SVD on filtered force data, get the prime direction
    Eigen::JacobiSVD<MatrixXd> svd_f(f_data_filtered.transpose(), Eigen::ComputeThinV);
    Vector3d F_T = svd_f.matrixV().col(0).normalized();
    // check sign
    MatrixXd check_sum = F_T.transpose() * f_data_filtered * VectorXd::Ones(f_data_length);
    if (check_sum(0,0) < 0) F_T = -F_T;
    MatrixXd Nf = F_T.transpose();

    /**
     * Infer Goal
     */
    // Transform Nf to world frame
    _F_W = R_WT * F_T;
    cout << "F_W: " << _F_W.transpose().format(MatlabFmt) << endl;
    Vector3d goal_direction_W = _F_W.cross(-Vector3d::UnitX());
    cout << "goal_direction_W: " << goal_direction_W.transpose().format(MatlabFmt) << endl;

    MatrixXd G = MatrixXd::Zero(1, 3);
    VectorXd b_G = VectorXd::Zero(1);
    G = (R_WT.transpose() * goal_direction_W).transpose();
    b_G(0) = _kGoalVelocityM;

    /**
     * Do Hybrid Servoing (3D)
     */
    HFVC action;
    int kDimActualized      = 3;
    int kDimUnActualized    = 0;
    int kDimSlidingFriction = 0;
    int kNumSeeds           = 3;
    int kDimLambda          = 1;
    int kPrintLevel         = 2;

    VectorXd F = VectorXd::Zero(3);
    MatrixXd Aeq(0, 1+3); // dummy
    VectorXd beq(0); // dummy
    MatrixXd A = MatrixXd::Zero(1, 1 + 3);
    VectorXd b_A = VectorXd::Zero(1);
    A(0, 0) = -1;
    b_A(0) = -_kNormalForceMin;

    cout << "Nf: " << Nf.format(MatlabFmt) << endl;
    cout << "G: " << G.format(MatlabFmt) << endl;
    cout << "b_G: " << b_G.format(MatlabFmt) << endl;
    cout << "F: " << F.format(MatlabFmt) << endl;
    cout << "Aeq: " << Aeq.format(MatlabFmt) << endl;
    cout << "beq: " << beq.format(MatlabFmt) << endl;
    cout << "A: " << A.format(MatlabFmt) << endl;
    cout << "b_A: " << b_A.format(MatlabFmt) << endl;

    solvehfvc(Nf, G, b_G, F, Aeq, beq, A, b_A,
      kDimActualized, kDimUnActualized,
      kDimSlidingFriction, kDimLambda,
      kNumSeeds, kPrintLevel,
      &action);

    double computation_time_ms = timer.toc();

    /**
     * Execute the hybrid action (6D)
     */
    Vector6d v_Tr = Vector6d::Zero();
    for (int i = 0; i < action.n_av; ++i)  v_Tr(i+action.n_af) = action.w_av(i);

    // Tool frame velocity
    Matrix6d R_a;
    R_a << action.R_a, Matrix3d::Zero(), Matrix3d::Zero(), Matrix3d::Identity();
    Matrix6d R_a_inv = R_a.inverse();
    Vector6d v_T = R_a_inv*v_Tr;

    // World frame velocity, set pose
    _robot.getPose(pose_fb);
    Matrix4d SE3_WT_fb = posemm2SE3(pose_fb);
    Matrix6d Adj_WT = SE32Adj(SE3_WT_fb);
    Vector6d v_W = Adj_WT*v_T;
    Matrix4d SE3_WT_command;
    SE3_WT_command = SE3_WT_fb + wedge6(v_W)*SE3_WT_fb*_kTimeStepSec;
    SE32Posemm(SE3_WT_command, pose_set);

    // Tool frame force
    Vector6d force_Tr_set = Vector6d::Zero();
    for (int i = 0; i < action.n_af; ++i)  force_Tr_set[i] = action.eta_af(i);
    Vector6d force_T = R_a_inv*force_Tr_set;
    Matrix6d Adj_TW = SE32Adj(SE3Inv(SE3_WT_fb));

    Vector6d force_W = Adj_TW.transpose() * force_T;
    cout << "force_Tr_set: " << force_Tr_set.transpose().format(MatlabFmt) << endl;
    cout << "force_T: " << force_T.transpose().format(MatlabFmt) << endl;
    printf("V in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", v_W[0], v_W[1],
        v_W[2],v_W[3],v_W[4],v_W[5]);
    printf("F in world: %.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", force_W[0],
        force_W[1], force_W[2],force_W[3],force_W[4],force_W[5]);
    cout << "Computation Time: " << computation_time_ms << endl << endl;

    if (fr == _kNumOfTimeSteps-1) {
      // print to file
      std::ofstream fp;
      // f_queue
      fp.open(_task_data_file_path + "f_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_queue.size(); ++i) {
        stream_array_in(fp, _controller._f_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // v_queue
      fp.open(_task_data_file_path + "v_queue.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_queue.size(); ++i) {
        stream_array_in(fp, _controller._v_queue[i].data(), 3);
        fp << endl;
      }
      fp.close();
      // f_weights
      fp.open(_task_data_file_path + "f_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._f_weights.size(); ++i)
        fp << _controller._f_weights[i] << endl;
      fp.close();

      // v_weights
      fp.open(_task_data_file_path + "v_weights.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      for (int i = 0; i < _controller._v_weights.size(); ++i)
        fp << _controller._v_weights[i] << endl;
      fp.close();

      // f_data_filtered
      fp.open(_task_data_file_path + "f_data_filtered.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_filtered.transpose() << endl;
      fp.close();
      // f_data_selected
      fp.open(_task_data_file_path + "f_data_selected.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << f_data_selected.transpose() << endl;
      fp.close();
      // others
      fp.open(_task_data_file_path + "process.txt");
      if (!fp) {
        cerr << "Unable to open file to write.";
        return false;
      }
      fp << DimV << endl << DimF << endl;
      stream_array_in(fp, v_T.data(), 3);
      fp << endl;
      stream_array_in(fp, force_T.data(), 3);
      fp.close();
    }

    if (std::isnan(force_Tr_set[0])) {
      cout << "================== NaN =====================" << endl;
      cout << "Press Enter to continue.." << endl;
      getchar();
    }

    cout << "Current pose: " << pose_fb[0] << ", " << pose_fb[1] << ", " << pose_fb[2] << endl;
    cout << "Set pose: " << pose_set[0] << ", " << pose_set[1] << ", " << pose_set[2] << endl;
    cout << "Press Enter to begin motion: \n";
    // getchar();
    // cout << "motion begins:" << endl;
    _controller.ExecuteHFVC(action.n_af, action.n_av+3,
        R_a, pose_set, force_Tr_set.data(),
        HS_CONTINUOUS, _main_loop_rate, _kTimeStepSec);

  } // end for
  return true;
}


#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include "bhtsne.h"

using namespace std;
namespace po = boost::program_options;
namespace fsys = boost::filesystem;

int main(int argc, char **argv) {
  // Declare the supported options
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("input-P", po::value<string>()->value_name("FILE")->default_value("P.dat"), "name of binary input file containing P matrix (see ComputeP)")
    ("out-dir", po::value<string>()->value_name("DIR")->default_value("out"), "where to create output files; directory will be created if it does not exist")
    ("out-dim", po::value<int>()->value_name("NUM")->default_value(2), "number of output dimensions")
    ("max-iter", po::value<int>()->value_name("NUM")->default_value(1000), "maximum number of iterations")
    ("rand-seed", po::value<int>()->value_name("NUM")->default_value(-1), "seed for random number generator; to use current time as seed set it to -1")
    ("theta", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "a value between 0 and 1 that controls the accuracy-efficiency tradeoff in SPTree for gradient computation; 0 means exact")
    ("learn-rate", po::value<double>()->value_name("NUM")->default_value(200, "200"), "learning rate for gradient steps")
    ("mom-init", po::value<double>()->value_name("NUM")->default_value(0.5, "0.5"), "initial momentum between 0 and 1")
    ("mom-final", po::value<double>()->value_name("NUM")->default_value(0.8, "0.8"), "final momentum between 0 and 1 (switch point controlled by --mom-switch-iter)")
    ("mom-switch-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of initial momentum")
    ("early-exag-iter", po::value<int>()->value_name("NUM")->default_value(250), "duration (number of iterations) of early exaggeration")
    ("skip-random-init", po::bool_switch()->default_value(false), "skip random initialization")
    ("batch-frac", po::value<double>()->value_name("NUM"), "what fraction of points to update for each iteration")
    ("cache-iter", po::value<int>()->value_name("NUM")->default_value(INT_MAX, "INT_MAX"), "After every NUM iterations, write intermediary embeddings to disk. Final embedding is always reported.")
  ;

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

  if (vm.count("help")) {
    cout << "Usage: RunBhtsne [options]" << endl;
    cout << desc << "\n";
    return 1;
  }

  string infile = vm["input-P"].as<string>();
  string outdir = vm["out-dir"].as<string>();

  fsys::path dir(outdir);
  if (fsys::is_directory(dir)) {
    cout << "Error: Output directory already exists" << endl;
    return 1;
  }
  if (fsys::create_directory(dir)) {
    cout << "Output directory created: " << outdir << endl;
  }

  TSNE *tsne = new TSNE();

  fsys::path paramfile = dir;
  paramfile /= "param.txt";
  ofstream ofs(paramfile.string().c_str());
  ofs << "input-P: " << infile << endl;
  ofs << "out-dir: " << fsys::canonical(dir).string() << endl;

  int no_dims = vm["out-dim"].as<int>(); ofs << "out-dim: " << no_dims << endl;
  double theta = vm["theta"].as<double>(); ofs << "theta: " << theta << endl;
  int rand_seed = vm["rand-seed"].as<int>(); ofs << "rand-seed: " << rand_seed << endl;
  bool skip_random_init = vm["skip-random-init"].as<bool>(); ofs << "skip-random-init: " << skip_random_init << endl;
  int max_iter = vm["max-iter"].as<int>(); ofs << "max-iter: " << max_iter << endl;
  int stop_lying_iter = vm["early-exag-iter"].as<int>(); ofs << "early-exag-iter: " << stop_lying_iter << endl;
  int mom_switch_iter = vm["mom-switch-iter"].as<int>(); ofs << "mom-switch-iter: " << mom_switch_iter << endl;
  double momentum = vm["mom-init"].as<double>(); ofs << "mom-init: " << momentum << endl;
  double final_momentum = vm["mom-final"].as<double>(); ofs << "mom-final: " << final_momentum << endl;
  double eta = vm["learn-rate"].as<double>(); ofs << "learn-rate: " << eta << endl;
  tsne->CACHE_ITER = vm["cache-iter"].as<int>(); ofs << "cache-iter: " << tsne->CACHE_ITER << endl;

  if (vm.count("batch-frac")) {
    double batch_frac = vm["batch-frac"].as<double>(); ofs << "batch-frac: " << batch_frac << endl;
    tsne->BATCH_FLAG = true;
    tsne->BATCH_FRAC = batch_frac;
  }

  ofs.close();

  printf("Loading input similarities...\n");
  int N;
  unsigned int *row_P;
  unsigned int *col_P;
  double *val_P;
  if (!tsne->load_P(infile, N, &row_P, &col_P, &val_P)) {
    cout << "Error: failed to load P from " << infile << endl;
    return 1;
  }

  double *Y = (double *)malloc(N * no_dims * sizeof(double));
  if (Y == NULL) {
    cout << "Error: Memory allocation for the output failed" << endl;
    return 1;
  }

  if (!tsne->run(N, row_P, col_P, val_P, Y, no_dims, theta, rand_seed,
           skip_random_init, max_iter, stop_lying_iter, mom_switch_iter,
           momentum, final_momentum, eta, dir)) {
    return 1;
  }

  free(row_P);
  free(col_P);
  free(val_P);
  free(Y);
  delete(tsne);

  cout << "Done" << endl;

  return 0;
}


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

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_STRATEGY_TRANSFORM_HPP
#define BOOST_GEOMETRY_STRATEGIES_STRATEGY_TRANSFORM_HPP

#include <cstddef>
#include <cmath>
#include <functional>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/convert.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/transform.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace transform
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template
<
    typename Src, typename Dst,
    std::size_t D, std::size_t N,
    template <typename> class F
>
struct transform_coordinates
{
    template <typename T>
    static inline void transform(Src const& source, Dst& dest, T value)
    {
        typedef typename select_coordinate_type<Src, Dst>::type coordinate_type;

        F<coordinate_type> function;
        set<D>(dest, boost::numeric_cast<coordinate_type>(function(get<D>(source), value)));
        transform_coordinates<Src, Dst, D + 1, N, F>::transform(source, dest, value);
    }
};

template
<
    typename Src, typename Dst,
    std::size_t N,
    template <typename> class F
>
struct transform_coordinates<Src, Dst, N, N, F>
{
    template <typename T>
    static inline void transform(Src const& , Dst& , T )
    {
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief Transformation strategy to copy one point to another using assignment operator
    \ingroup transform
    \tparam P point type
 */
template <typename P>
struct copy_direct
{
    inline bool apply(P const& p1, P& p2) const
    {
        p2 = p1;
        return true;
    }
};

/*!
    \brief Transformation strategy to do copy a point, copying per coordinate.
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct copy_per_coordinate
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        // Defensive check, dimensions are equal, selected by specialization
        assert_dimension_equal<P1, P2>();

        geometry::convert(p1, p2);
        return true;
    }
};

/*!
    \brief Transformation strategy to go from degree to radian and back
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
    \tparam F additional functor to divide or multiply with d2r
 */
template <typename P1, typename P2, template <typename> class F>
struct degree_radian_vv
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        // Spherical coordinates always have 2 coordinates measured in angles
        // The optional third one is distance/height, provided in another strategy
        // Polar coordinates having one angle, will be also in another strategy
        assert_dimension<P1, 2>();
        assert_dimension<P2, 2>();

        detail::transform_coordinates<P1, P2, 0, 2, F>::transform(p1, p2, math::d2r);
        return true;
    }
};

template <typename P1, typename P2, template <typename> class F>
struct degree_radian_vv_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        assert_dimension<P2, 3>();

        detail::transform_coordinates<P1, P2, 0, 2, F>::transform(p1, p2, math::d2r);
        // Copy height or other third dimension
        set<2>(p2, get<2>(p1));
        return true;
    }
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

    /// Helper function for conversion, phi/theta are in radians
    template <typename P, typename T, typename R>
    inline void spherical_polar_to_cartesian(T phi, T theta, R r, P& p)
    {
        assert_dimension<P, 3>();

        // http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_spherical_coordinates
        // http://www.vias.org/comp_geometry/math_coord_convert_3d.htm
        // https://moodle.polymtl.ca/file.php/1183/Autres_Documents/Derivation_for_Spherical_Co-ordinates.pdf
        // http://en.citizendium.org/wiki/Spherical_polar_coordinates

        // Phi = first, theta is second, r is third, see documentation on cs::spherical

        // (calculations are splitted to implement ttmath)

        T r_sin_theta = r;
        T r_cos_theta = r;
        r_sin_theta *= sin(theta);
        r_cos_theta *= cos(theta);

        set<0>(p, r_sin_theta * cos(phi));
        set<1>(p, r_sin_theta * sin(phi));
        set<2>(p, r_cos_theta);
    }

    /// Helper function for conversion, lambda/delta (lon lat) are in radians
    template <typename P, typename T, typename R>
    inline void spherical_equatorial_to_cartesian(T lambda, T delta, R r, P& p)
    {
        assert_dimension<P, 3>();

        // http://mathworld.wolfram.com/GreatCircle.html
        // http://www.spenvis.oma.be/help/background/coortran/coortran.html WRONG

        T r_cos_delta = r;
        T r_sin_delta = r;
        r_cos_delta *= cos(delta);
        r_sin_delta *= sin(delta);

        set<0>(p, r_cos_delta * cos(lambda));
        set<1>(p, r_cos_delta * sin(lambda));
        set<2>(p, r_sin_delta);
    }

    /// Helper function for conversion
    template <typename P, typename T>
    inline bool cartesian_to_spherical2(T x, T y, T z, P& p)
    {
        assert_dimension<P, 2>();

        // http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_Cartesian_coordinates

#if defined(BOOST_GEOMETRY_TRANSFORM_CHECK_UNIT_SPHERE)
        // TODO: MAYBE ONLY IF TO BE CHECKED?
        T const r = /*sqrt not necessary, sqrt(1)=1*/ (x * x + y * y + z * z);

        // Unit sphere, so r should be 1
        if (geometry::math::abs(r - 1.0) > T(1e-6))
        {
            return false;
        }
        // end todo
#endif

        set_from_radian<0>(p, atan2(y, x));
        set_from_radian<1>(p, acos(z));
        return true;
    }

    template <typename P, typename T>
    inline bool cartesian_to_spherical_equatorial2(T x, T y, T z, P& p)
    {
        assert_dimension<P, 2>();

        set_from_radian<0>(p, atan2(y, x));
        set_from_radian<1>(p, asin(z));
        return true;
    }

    template <typename P, typename T>
    inline bool cartesian_to_spherical3(T x, T y, T z, P& p)
    {
        assert_dimension<P, 3>();

        // http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_Cartesian_coordinates
        T const r = sqrt(x * x + y * y + z * z);
        set<2>(p, r);
        set_from_radian<0>(p, atan2(y, x));
        if (r > 0.0)
        {
            set_from_radian<1>(p, acos(z / r));
            return true;
        }
        return false;
    }

	template <typename P, typename T>
	inline bool cartesian_to_spherical_equatorial3(T x, T y, T z, P& p)
	{
		assert_dimension<P, 3>();

		// http://en.wikipedia.org/wiki/List_of_canonical_coordinate_transformations#From_Cartesian_coordinates
		T const r = sqrt(x * x + y * y + z * z);
		set<2>(p, r);
		set_from_radian<0>(p, atan2(y, x));
		if (r > 0.0)
		{
			set_from_radian<1>(p, asin(z / r));
			return true;
		}
		return false;
	}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief Transformation strategy for 2D spherical (phi,theta) to 3D cartesian (x,y,z)
    \details on Unit sphere
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct from_spherical_polar_2_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 2>();
        detail::spherical_polar_to_cartesian(get_as_radian<0>(p1), get_as_radian<1>(p1), 1.0, p2);
        return true;
    }
};

template <typename P1, typename P2>
struct from_spherical_equatorial_2_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 2>();
        detail::spherical_equatorial_to_cartesian(get_as_radian<0>(p1), get_as_radian<1>(p1), 1.0, p2);
        return true;
    }
};

/*!
    \brief Transformation strategy for 3D spherical (phi,theta,r) to 3D cartesian (x,y,z)
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct from_spherical_polar_3_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        detail::spherical_polar_to_cartesian(
                    get_as_radian<0>(p1), get_as_radian<1>(p1), get<2>(p1), p2);
        return true;
    }
};

template <typename P1, typename P2>
struct from_spherical_equatorial_3_to_cartesian_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        detail::spherical_equatorial_to_cartesian(
                    get_as_radian<0>(p1), get_as_radian<1>(p1), get<2>(p1), p2);
        return true;
    }
};

/*!
    \brief Transformation strategy for 3D cartesian (x,y,z) to 2D spherical (phi,theta)
    \details on Unit sphere
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
    \note If x,y,z point is not lying on unit sphere, transformation will return false
 */
template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_polar_2
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        return detail::cartesian_to_spherical2(get<0>(p1), get<1>(p1), get<2>(p1), p2);
    }
};

template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_equatorial_2
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        return detail::cartesian_to_spherical_equatorial2(get<0>(p1), get<1>(p1), get<2>(p1), p2);
    }
};

/*!
    \brief Transformation strategy for 3D cartesian (x,y,z) to 3D spherical (phi,theta,r)
    \ingroup transform
    \tparam P1 first point type
    \tparam P2 second point type
 */
template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_polar_3
{
    inline bool apply(P1 const& p1, P2& p2) const
    {
        assert_dimension<P1, 3>();
        return detail::cartesian_to_spherical3(get<0>(p1), get<1>(p1), get<2>(p1), p2);
    }
};

template <typename P1, typename P2>
struct from_cartesian_3_to_spherical_equatorial_3
{
	inline bool apply(P1 const& p1, P2& p2) const
	{
		assert_dimension<P1, 3>();
		return detail::cartesian_to_spherical_equatorial3(get<0>(p1), get<1>(p1), get<2>(p1), p2);
	}
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

/// Specialization for same coordinate system family, same system, same dimension, same point type, can be copied
template <typename CoordSysTag, typename CoordSys, std::size_t D, typename P>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys, CoordSys, D, D, P, P>
{
    typedef copy_direct<P> type;
};

/// Specialization for same coordinate system family and system, same dimension, different point type, copy per coordinate
template <typename CoordSysTag, typename CoordSys, std::size_t D, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys, CoordSys, D, D, P1, P2>
{
    typedef copy_per_coordinate<P1, P2> type;
};

/// Specialization to transform from degree to radian for any coordinate system / point type combination
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<degree>, CoordSys<radian>, 2, 2, P1, P2>
{
    typedef degree_radian_vv<P1, P2, std::multiplies> type;
};

/// Specialization to transform from radian to degree for any coordinate system / point type combination
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<radian>, CoordSys<degree>, 2, 2, P1, P2>
{
    typedef degree_radian_vv<P1, P2, std::divides> type;
};

/// Specialization degree->radian in 3D
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<degree>, CoordSys<radian>, 3, 3, P1, P2>
{
    typedef degree_radian_vv_3<P1, P2, std::multiplies> type;
};

/// Specialization radian->degree in 3D
template <typename CoordSysTag, template<typename> class CoordSys, typename P1, typename P2>
struct default_strategy<CoordSysTag, CoordSysTag, CoordSys<radian>, CoordSys<degree>, 3, 3, P1, P2>
{
    typedef degree_radian_vv_3<P1, P2, std::divides> type;
};

/// Specialization to transform from unit sphere(phi,theta) to XYZ
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_polar_tag, cartesian_tag, CoordSys1, CoordSys2, 2, 3, P1, P2>
{
    typedef from_spherical_polar_2_to_cartesian_3<P1, P2> type;
};

/// Specialization to transform from sphere(phi,theta,r) to XYZ
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_polar_tag, cartesian_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
    typedef from_spherical_polar_3_to_cartesian_3<P1, P2> type;
};

template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_equatorial_tag, cartesian_tag, CoordSys1, CoordSys2, 2, 3, P1, P2>
{
    typedef from_spherical_equatorial_2_to_cartesian_3<P1, P2> type;
};

template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<spherical_equatorial_tag, cartesian_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
    typedef from_spherical_equatorial_3_to_cartesian_3<P1, P2> type;
};

/// Specialization to transform from XYZ to unit sphere(phi,theta)
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_polar_tag, CoordSys1, CoordSys2, 3, 2, P1, P2>
{
    typedef from_cartesian_3_to_spherical_polar_2<P1, P2> type;
};

template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_equatorial_tag, CoordSys1, CoordSys2, 3, 2, P1, P2>
{
    typedef from_cartesian_3_to_spherical_equatorial_2<P1, P2> type;
};

/// Specialization to transform from XYZ to sphere(phi,theta,r)
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_polar_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
    typedef from_cartesian_3_to_spherical_polar_3<P1, P2> type;
};
template <typename CoordSys1, typename CoordSys2, typename P1, typename P2>
struct default_strategy<cartesian_tag, spherical_equatorial_tag, CoordSys1, CoordSys2, 3, 3, P1, P2>
{
	typedef from_cartesian_3_to_spherical_equatorial_3<P1, P2> type;
};

} // namespace services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::transform

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_STRATEGY_TRANSFORM_HPP


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

#ifndef CRYPTO3_PUBKEY_MODES_CREATE_KEY_HPP
#define CRYPTO3_PUBKEY_MODES_CREATE_KEY_HPP

#include <type_traits>
#include <iterator>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

#include <nil/crypto3/pubkey/secret_sharing.hpp>
#include <nil/crypto3/pubkey/dkg.hpp>

#include <nil/crypto3/pubkey/private_key.hpp>

#include <nil/crypto3/pubkey/algorithm/deal_shares.hpp>
#include <nil/crypto3/pubkey/algorithm/deal_share.hpp>
#include <nil/crypto3/pubkey/algorithm/verify_share.hpp>

namespace nil {
    namespace crypto3 {
        //
        // CoeffsIterator - coefficients of polynomial
        //
        template<typename Scheme, typename CoeffsIterator, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<CoeffsIterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::shamir_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value ||
                std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(CoeffsIterator first, CoeffsIterator last, Number n) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIterator>));

            using privkeys_type = std::vector<pubkey::private_key<Scheme>>;
            using sss_no_key_ops_type = typename pubkey::private_key<Scheme>::sss_public_key_no_key_ops_type;

            typename sss_no_key_ops_type::shares_type shares =
                nil::crypto3::deal_shares<SecretSharingScheme>(first, last, n);
            privkeys_type privkeys;
            for (const auto &s : shares) {
                privkeys.emplace_back(s);
            }
            auto PK = pubkey::public_key<Scheme>(sss_no_key_ops_type::get_public_coeffs(first, last).front());
            return std::make_pair(PK, privkeys);
        }

        //
        // CoeffsRange - coefficients of polynomial
        //
        template<typename Scheme, typename CoeffsRange, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<typename CoeffsRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::shamir_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value ||
                std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(const CoeffsRange &r, Number n) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const CoeffsRange>));
            return create_key<Scheme>(r.begin(), r.end(), n);
        }

        //
        // InputIterator - public representation values of polynomial's coefficients
        //
        template<typename Scheme, typename CoeffsIterator, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<CoeffsIterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            pubkey::private_key<Scheme>>::type
            create_key(CoeffsIterator first,
                       CoeffsIterator last,
                       typename SecretSharingScheme::share_type share,
                       Number n) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIterator>));

            using privkey_type = pubkey::private_key<Scheme>;

            assert(static_cast<bool>(nil::crypto3::verify_share<SecretSharingScheme>(first, last, share)));
            return privkey_type(share);
        }

        //
        // PublicCoeffsRange - public representation values of polynomial's coefficients
        //
        template<typename Scheme, typename PublicCoeffsRange, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType = typename std::iterator_traits<typename PublicCoeffsRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::feldman_sss<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            pubkey::private_key<Scheme>>::type
            create_key(const PublicCoeffsRange &r, typename SecretSharingScheme::share_type share, Number n) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const PublicCoeffsRange>));
            return create_key<Scheme>(r.begin(), r.end(), share, n);
        }

        //
        // CoeffsIterator - coefficients of polynomial
        // InputIterator2 - participants' weights
        //
        template<typename Scheme, typename CoeffsIterator, typename WeightsIterator,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<CoeffsIterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<WeightsIterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_weight_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::weighted_shamir_sss<typename SecretSharingScheme::group_type>,
                         SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(CoeffsIterator first1, CoeffsIterator last1, WeightsIterator first2, WeightsIterator last2) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<CoeffsIterator>));
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<WeightsIterator>));

            using privkeys_type = std::vector<pubkey::private_key<Scheme>>;
            using sss_no_key_ops_type = typename pubkey::private_key<Scheme>::sss_public_key_no_key_ops_type;

            typename sss_no_key_ops_type::shares_type shares = nil::crypto3::deal_shares<SecretSharingScheme>(
                first1, last1, first2, last2, std::distance(first2, last2));
            privkeys_type privkeys;
            for (const auto &s : shares) {
                privkeys.emplace_back(s, std::distance(first1, last1));
            }
            auto PK = pubkey::public_key<Scheme>(sss_no_key_ops_type::get_public_coeffs(first1, last1).front(),
                                                 std::distance(first2, last2));
            return std::make_pair(PK, privkeys);
        }

        //
        // CoeffsRange - coefficients of polynomial
        // WeightsRange - participants' weights
        //
        template<typename Scheme, typename CoeffsRange, typename WeightsRange,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<typename CoeffsRange::iterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<typename WeightsRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_weight_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::weighted_shamir_sss<typename SecretSharingScheme::group_type>,
                         SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, std::vector<pubkey::private_key<Scheme>>>>::type
            create_key(const CoeffsRange &r1, const WeightsRange &r2) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const CoeffsRange>));
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const WeightsRange>));
            return create_key<Scheme>(r1.begin(), r1.end(), r2.begin(), r2.end());
        }

        //
        // PublicCoeffsIterators - public representation values of polynomials' coefficients of other participants
        // SharesIterator - shares generated by other participants
        //
        template<typename Scheme, typename PublicCoeffsIterators, typename SharesIterator, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<
                     typename std::iterator_traits<PublicCoeffsIterators>::value_type::iterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<SharesIterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_share_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::pedersen_dkg<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, pubkey::private_key<Scheme>>>::type
            create_key(PublicCoeffsIterators first1, PublicCoeffsIterators last1, SharesIterator first2,
                       SharesIterator last2, Number n) {
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<PublicCoeffsIterators>));
            BOOST_RANGE_CONCEPT_ASSERT((
                boost::SinglePassRangeConcept<const typename std::iterator_traits<PublicCoeffsIterators>::value_type>));
            BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<SharesIterator>));
            assert(n == std::distance(first1, last1));
            assert(n == std::distance(first2, last2));

            using privkey_type = pubkey::private_key<Scheme>;

            auto it_coeffs = first1;
            auto it_share = first2;
            typename SecretSharingScheme::public_element_type PK = SecretSharingScheme::public_element_type::zero();
            while (it_coeffs != last1 && it_share != last2) {
                assert(static_cast<bool>(nil::crypto3::verify_share<SecretSharingScheme>(*it_coeffs, *it_share)));
                PK = PK + *((*it_coeffs).begin());
                it_coeffs++;
                it_share++;
            }
            return std::make_pair(pubkey::public_key<Scheme>(PK),
                                  privkey_type(static_cast<typename SecretSharingScheme::share_type>(
                                      nil::crypto3::deal_share<SecretSharingScheme>(first2, last2))));
        }

        //
        // PublicCoeffsRanges - public representation values of polynomials' coefficients of other participants
        // SharesRange - shares generated by other participants
        //
        template<typename Scheme, typename PublicCoeffsRanges, typename SharesRange, typename Number,
                 typename SecretSharingScheme = typename pubkey::private_key<Scheme>::sss_public_key_group_type,
                 typename ValueType1 = typename std::iterator_traits<typename std::iterator_traits<
                     typename PublicCoeffsRanges::iterator>::value_type::iterator>::value_type,
                 typename ValueType2 = typename std::iterator_traits<typename SharesRange::iterator>::value_type,
                 typename SecretSharingScheme::template check_public_coeff_type<ValueType1> = true,
                 typename SecretSharingScheme::template check_share_type<ValueType2> = true>
        inline typename std::enable_if<
            std::is_same<pubkey::pedersen_dkg<typename SecretSharingScheme::group_type>, SecretSharingScheme>::value,
            std::pair<pubkey::public_key<Scheme>, pubkey::private_key<Scheme>>>::type
            create_key(const PublicCoeffsRanges &r, const SharesRange &shares, Number n) {
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const PublicCoeffsRanges>));
            BOOST_RANGE_CONCEPT_ASSERT(
                (boost::SinglePassRangeConcept<
                    const typename std::iterator_traits<typename PublicCoeffsRanges::iterator>::value_type>));
            BOOST_RANGE_CONCEPT_ASSERT((boost::SinglePassRangeConcept<const SharesRange>));
            return create_key<Scheme>(r.begin(), r.end(), shares.begin(), shares.end(), n);
        }
    }    // namespace crypto3
}    // namespace nil

#endif    // include guard


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include "convert.h"
#include "traits.h"

#include <comma/csv/stream.h>
#include <Eigen/Dense>

namespace {

    using namespace snark::imaging;

    typedef std::vector< double > channels;

    // conversion does not have to be a linear operation, but many are; these function simplify setting up such conversions
    /*
      commented-out to remove a compiler warning (-Wunused-function)
    channels linear_combination( const channels & i, const Eigen::Vector3d & before, const Eigen::Matrix3d & m, const Eigen::Vector3d & after )
    {
        channels t = { i[0] + before(0), i[1] + before(1), i[2] + before(2) };
        return channels( { m(0,0) * t[0] + m(0,1) * t[1] + m(0,2) * t[2] + after(0)
                         , m(1,0) * t[0] + m(1,1) * t[1] + m(1,2) * t[2] + after(1)
                         , m(2,0) * t[0] + m(2,1) * t[1] + m(2,2) * t[2] + after(2) } );
    }

    channels linear_combination( const channels & i, const Eigen::Vector3d & before, const Eigen::Matrix3d & m )
    {
        channels t = { i[0] + before(0), i[1] + before(1), i[2] + before(2) };
        return channels( { m(0,0) * t[0] + m(0,1) * t[1] + m(0,2) * t[2]
                         , m(1,0) * t[0] + m(1,1) * t[1] + m(1,2) * t[2]
                         , m(2,0) * t[0] + m(2,1) * t[1] + m(2,2) * t[2] } );
    }
    */
    channels linear_combination( const channels & i, const Eigen::Matrix3d & m, const Eigen::Vector3d & after )
    {
        return channels( { m(0,0) * i[0] + m(0,1) * i[1] + m(0,2) * i[2] + after(0)
                         , m(1,0) * i[0] + m(1,1) * i[1] + m(1,2) * i[2] + after(1)
                         , m(2,0) * i[0] + m(2,1) * i[1] + m(2,2) * i[2] + after(2) } );
    }

    channels linear_combination( const channels & i, const Eigen::Matrix3d & m )
    {
        return channels( { m(0,0) * i[0] + m(0,1) * i[1] + m(0,2) * i[2]
                         , m(1,0) * i[0] + m(1,1) * i[1] + m(1,2) * i[2]
                         , m(2,0) * i[0] + m(2,1) * i[1] + m(2,2) * i[2] } );
    }

    typedef std::function< channels ( const channels & p ) > C;
    // TODO:
    // - outr can be duplicate to the explicit convert call in convert (or the other way around); sort out
    typedef std::pair< colorspace::cspace, range > half_key_t;
    typedef std::pair< half_key_t, half_key_t > conversion_key_t;
    typedef std::map< conversion_key_t, C > conversion_map_t;

    C scale( range inr, range outr )
    {
        double uin, lin, uout, lout;
        switch ( inr ) {
            case ub : uin = limits< ub >::upper(); lin = limits< ub >::lower(); break;
            case uw : uin = limits< uw >::upper(); lin = limits< uw >::lower(); break;
            case ui : uin = limits< ui >::upper(); lin = limits< ui >::lower(); break;
            case f  : uin = limits< f  >::upper(); lin = limits< f  >::lower(); break;
            case d  : uin = limits< d  >::upper(); lin = limits< d  >::lower(); break;
            default:
                COMMA_THROW( comma::exception, "unknown input range '" << stringify::from( inr ) << "'" );
        }

        switch ( outr ) {
            case ub : uout = limits< ub >::upper(); lout = limits< ub >::lower(); break;
            case uw : uout = limits< uw >::upper(); lout = limits< uw >::lower(); break;
            case ui : uout = limits< ui >::upper(); lout = limits< ui >::lower(); break;
            case f  : uout = limits< f  >::upper(); lout = limits< f  >::lower(); break;
            case d  : uout = limits< d  >::upper(); lout = limits< d  >::lower(); break;
            default:
                COMMA_THROW( comma::exception, "unknown output range '" << stringify::from( outr ) << "'" );
        }

        double factor = ( uout - lout ) / ( uin - lin );
        return [ factor, lin, lout ]( const channels & rhs ){
            return channels( { factor * ( rhs[0] - lin ) + lout
                             , factor * ( rhs[1] - lin ) + lout
                             , factor * ( rhs[2] - lin ) + lout } ); };
    }

    const conversion_map_t & conversions()
    {
        static conversion_map_t m;
        m[ std::make_pair( std::make_pair( colorspace::rgb, ub ), std::make_pair( colorspace::rgb,   ub ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 1, 0, 0, 0, 1, 0, 0, 0, 1).finished() ); };
        m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 0.299, 0.587, 0.114, -0.168736, -0.331264, 0.5, 0.5, -0.418688, -0.081312).finished(), (Eigen::Vector3d() << 0, 0.5, 0.5).finished() ); };
        m[ std::make_pair( std::make_pair( colorspace::rgb, d  ), std::make_pair( colorspace::ypbpr, d  ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ];
        m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, d  ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ];
        m[ std::make_pair( std::make_pair( colorspace::rgb, d  ), std::make_pair( colorspace::ypbpr, f  ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ypbpr, f  ) ) ];
        m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ycbcr, ub ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 65.481, 128.553, 24.966, -37.797, -74.203, 112.0, 112.0, -93.786, -18.214).finished(), (Eigen::Vector3d() << 16, 128, 128).finished() ); };
        m[ std::make_pair( std::make_pair( colorspace::rgb, d  ), std::make_pair( colorspace::ycbcr, ub ) ) ] = m[ std::make_pair( std::make_pair( colorspace::rgb, f  ), std::make_pair( colorspace::ycbcr, ub ) ) ];
        // this differs from commonly presented values, e.g., wiki@YCbCr page, that rounds numbers to 3 decimal digits
        m[ std::make_pair( std::make_pair( colorspace::rgb, ub ), std::make_pair( colorspace::ycbcr, ub ) ) ] =
            []( const channels & i ){ return linear_combination( i, (Eigen::Matrix3d() << 65.481, 128.553, 24.966, -37.797, -74.203, 112.0, 112.0, -93.786, -18.214).finished() / 255., (Eigen::Vector3d() << 16, 128, 128).finished() ); };
        {
            m[ std::make_pair( std::make_pair( colorspace::ycbcr, ub ), std::make_pair( colorspace::rgb, ub ) ) ] =
                []( const channels & i) {
                    return channels( { 255/219. * ( i[0] - 16 )                                               + 255/112.*0.701             * ( i[2] - 128 )
                                     , 255/219. * ( i[0] - 16 ) - 255/112.*0.886*0.114/0.587 * ( i[1] - 128 ) - 255/112.*0.701*0.299/0.587 * ( i[2] - 128 )
                                     , 255/219. * ( i[0] - 16 ) + 255/112.*0.886             * ( i[1] - 128 )                                               } );
                };
        }
        return m;
    }

    C select( const conversion_map_t & m, const colorspace & inc, range inr, const colorspace & outc, range outr, bool recurse = true )
    {
        // the logic for searching for the conversion method:
        // 0. check for non-conversions: same colorspace, different (or even same) range; handle explicitly
        if ( inc.value == outc.value )
        {
            if ( inr == outr ) { return []( const channels & c ) { return c; }; }
            return scale( inr, outr );
        }
        // 1. if there is a direct conversion from (inc, inr) to (outc, outr) use it (data are stored as doubles on both in and out)
        {
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, inr ), std::make_pair( outc.value, outr ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return i->second; }
        }
        // 2. if there is a conversion from (inc, inr) to (outc, default-range-of-outc) use it, then change range of outc (chain conversions)
        {
            range outdef = colorspace::default_range( outc.value );
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, inr ), std::make_pair( outc.value, outdef ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return [ i, outdef, outr ]( const channels & c ){ return scale( outdef, outr )( i->second( c ) ); }; }
        }
        // 3. if there is a conversion from (inc, default-range-of-inc) to (outc, outr), use it after changing range of inc
        {
            range indef = colorspace::default_range( inc.value );
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, indef ), std::make_pair( outc.value, outr ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return [ i, inr, indef ]( const channels & c ){ return i->second( scale( inr, indef )( c ) ); }; }
        }
        // 4. if there is a conversion from (inc, default-range-of-inc) to (outc, default-range-of-outc), change range of inc, apply conversion, then change range of outc
        {
            range indef = colorspace::default_range( inc.value );
            range outdef = colorspace::default_range( outc.value );
            const conversion_key_t & key = std::make_pair( std::make_pair( inc.value, indef ), std::make_pair( outc.value, outdef ) );
            conversion_map_t::const_iterator i = m.find( key );
            if ( i != m.end() ) { return [ i, inr, indef, outdef, outr ]( const channels & c ){ return scale( outdef, outr )( i->second( scale( inr, indef )( c ) ) ); }; }
        }
        // 5. if there is a conversion from (inc, inr ) to (rgb, some-range-of-rgb) and also conversion from (rgb, some-range-of-rgb) to (outc, outr)
        //    ( covering all possible intermediate conversions through a recursive call), convert through the intermediate rgb
        {
            if ( recurse )
            {
                for ( range r : { ub, f } )
                {
                    try {
                        // do not stuck in infinite recursion
                        C c0 = select( m, inc, inr, colorspace::rgb, r, false );
                        C c1 = select( m, colorspace::rgb, r, outc, outr, false );
                        return [ c0, c1 ]( const channels & c ){ return c1( c0( c ) ); };
                    }
                    catch ( comma::exception & )
                    {}
                }
            }
        }
        // 6. if all the above fails, throw
        COMMA_THROW( comma::exception, "conversion from colorspace " << inc << ", range " << stringify::from( inr ) << " to colorspace " << outc << ", range " << stringify::from( outr ) << " is not known" );
    }

    typedef std::function< void ( const comma::csv::options & csv, const C & c ) > F;

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, typename outt >
    void convert( const comma::csv::options & csv, const C & c )
    {
        pixel< double, inr > sample_in;
        comma::csv::input_stream< pixel< double, inr > > is( std::cin, csv, sample_in );
        comma::csv::options output_csv;
        output_csv.full_xpath = false;
        output_csv.flush = csv.flush;
        if( csv.binary() ) { output_csv.format( comma::csv::format::value< pixel< outt, outr > >() ); }
        pixel< outt, outr > sample_out;
        comma::csv::output_stream< pixel< outt, outr > > os( std::cout, output_csv, sample_out );
        comma::csv::tied< pixel< double, inr >, pixel< outt, outr > > tied( is, os );
        pixel< double, outr > op;
        while( is.ready() || std::cin.good() )
        {
            const pixel< double, inr > * ip = is.read();
            if( !ip ) { break; }
            c( ip->channel ).swap( op.channel );
            tied.append( pixel< outt, outr >( op ) );
            if ( output_csv.flush ) { std::cout.flush(); }
        }
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_integral< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_integral< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        if ( outr_v == ub && outt_v == ub ) { return convert< inc, inr, outc, ub, typename range_traits< ub >::value_t >; }
        if ( outr_v == ub && outt_v == uw ) { return convert< inc, inr, outc, ub, typename range_traits< uw >::value_t >; }
        if ( outr_v == ub && outt_v == ui ) { return convert< inc, inr, outc, ub, typename range_traits< ui >::value_t >; }
        if ( outr_v == uw && outt_v == uw ) { return convert< inc, inr, outc, uw, typename range_traits< uw >::value_t >; }
        if ( outr_v == uw && outt_v == ui ) { return convert< inc, inr, outc, uw, typename range_traits< ui >::value_t >; }
        if ( outr_v == ui && outt_v == ui ) { return convert< inc, inr, outc, ui, typename range_traits< ui >::value_t >; }
        COMMA_THROW( comma::exception, "unsupported combination of output range " << stringify::from( outr ) << " and type " << stringify::from( outt ) );
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_floating_point< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_integral< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        COMMA_THROW( comma::exception, "cannot use integer output type " << stringify::from( outt ) << " for " << stringify::from( outr ) << " output range" );
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_integral< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_floating_point< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        return convert< inc, inr, outc, outr, typename range_traits< outt >::value_t >;
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    F resolve_( typename std::enable_if< std::is_floating_point< typename range_traits< outr >::value_t >::value, typename range_traits< outr >::value_t >::type outr_v
              , typename std::enable_if< std::is_floating_point< typename range_traits< outt >::value_t >::value, typename range_traits< outt >::value_t >::type outt_v )
    {
        return convert< inc, inr, outc, outr, typename range_traits< outt >::value_t >;
    }

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr, range outt >
    struct outt_
    {
        static F dispatch()
        {
            // shall not attempt instantiating if:
            // - outr is integer, outt is integer and outr > outt
            // - outr is float, outt is integer
            typename range_traits< outr >::value_t outr_v( outr );
            typename range_traits< outt >::value_t outt_v( outt );
            return resolve_< inc, inr, outc, outr, outt >( outr_v, outt_v );
        }
    };

    template< colorspace::cspace inc, range inr, colorspace::cspace outc, range outr >
    struct outr_
    {
        static F dispatch( range outt )
        {
            switch ( outt )
            {
                case ub: return outt_< inc, inr, outc, outr, ub >::dispatch( ); break;
                case uw: return outt_< inc, inr, outc, outr, uw >::dispatch( ); break;
                case ui: return outt_< inc, inr, outc, outr, ui >::dispatch( ); break;
                case f:  return outt_< inc, inr, outc, outr, f  >::dispatch( ); break;
                case d:  return outt_< inc, inr, outc, outr, d  >::dispatch( ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown output storage format '" << stringify::from( outt ) << "'" );
            }
        }
    };

    template< colorspace::cspace inc, range inr, colorspace::cspace outc >
    struct outc_
    {
        static F dispatch( range outr, range outt )
        {
            switch ( outr )
            {
                case ub: return outr_< inc, inr, outc, ub >::dispatch( outt ); break;
                case uw: return outr_< inc, inr, outc, uw >::dispatch( outt ); break;
                case ui: return outr_< inc, inr, outc, ui >::dispatch( outt ); break;
                case f:  return outr_< inc, inr, outc, f  >::dispatch( outt ); break;
                case d:  return outr_< inc, inr, outc, d  >::dispatch( outt ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown output range '" << stringify::from( outr ) << "'" );
            }
        }
    };

    template< colorspace::cspace inc, range inr >
    struct inr_
    {
        static F dispatch( const colorspace & outc, range outr, range outt )
        {
            switch ( outc.value )
            {
                case colorspace::rgb:   return outc_< inc, inr, colorspace::rgb   >::dispatch( outr, outt ); break;
                case colorspace::ypbpr: return outc_< inc, inr, colorspace::ypbpr >::dispatch( outr, outt ); break;
                case colorspace::ycbcr: return outc_< inc, inr, colorspace::ycbcr >::dispatch( outr, outt ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown colorspace to '" << std::string( outc ) << "'" );
            }
        }
    };

    template< colorspace::cspace inc >
    struct inc_
    {
        static F dispatch( range inr, const colorspace & outc, range outr, range outt )
        {
            switch ( inr )
            {
                case ub: return inr_< inc, ub >::dispatch( outc, outr, outt ); break;
                case uw: return inr_< inc, uw >::dispatch( outc, outr, outt ); break;
                case ui: return inr_< inc, ui >::dispatch( outc, outr, outt ); break;
                case f:  return inr_< inc, f  >::dispatch( outc, outr, outt ); break;
                case d:  return inr_< inc, d  >::dispatch( outc, outr, outt ); break;
                default:
                    COMMA_THROW( comma::exception, "unknown input range '" << stringify::from( inr ) << "'" );
            }
        }
    };

} // anonymous

namespace snark { namespace imaging {

    converter::F converter::dispatch( const colorspace & inc, range inr, const colorspace & outc, range outr, range outt )
    {
        const C & conv = select( conversions(), inc, inr, outc, outr );

        switch ( inc.value )
        {
            case colorspace::rgb:   return std::bind( inc_< colorspace::rgb   >::dispatch( inr, outc, outr, outt ), std::placeholders::_1, conv ); break;
            case colorspace::ypbpr: return std::bind( inc_< colorspace::ypbpr >::dispatch( inr, outc, outr, outt ), std::placeholders::_1, conv ); break;
            case colorspace::ycbcr: return std::bind( inc_< colorspace::ycbcr >::dispatch( inr, outc, outr, outt ), std::placeholders::_1, conv ); break;
            default:
                COMMA_THROW( comma::exception, "unknown colorspace from '" << std::string( inc ) << "'" );
        }
    }

    void converter::list( std::ostream & os )
    {
        const auto & allc = { colorspace( colorspace::rgb ), colorspace( colorspace::ycbcr ), colorspace( colorspace::ypbpr ) };
        const auto & allr = { ub, uw, ui, f, d };
        for ( const auto & inc : allc )
        {
            for ( auto inr : allr )
            {
                for ( const auto & outc : allc )
                {
                    for ( auto outr : allr )
                    {
                        try
                        {
                            select( conversions(), inc, inr, outc, outr );
                            os << inc << ',' << stringify::from( inr ) << ',' << outc << ',' << stringify::from( outr ) << std::endl;
                        }
                        catch ( comma::exception & )
                        {}
                    }
                }
            }
        }
    }

} } // namespace snark { namespace imaging {


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include <memory>
#include <boost/bind.hpp>

#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h"
#include "Tudat/Astrodynamics/Aerodynamics/flightConditions.h"
#include "Tudat/Astrodynamics/Aerodynamics/standardAtmosphere.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/oblateSpheroidBodyShapeModel.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

namespace tudat
{

namespace aerodynamics
{

//! Constructor, sets objects and functions from which relevant environment and state variables are retrieved.
FlightConditions::FlightConditions( const std::shared_ptr< basic_astrodynamics::BodyShapeModel > shapeModel,
                  const std::shared_ptr< reference_frames::AerodynamicAngleCalculator >
                  aerodynamicAngleCalculator ):
    shapeModel_( shapeModel ),
    aerodynamicAngleCalculator_( aerodynamicAngleCalculator ),
    currentTime_( TUDAT_NAN )
{
    // Link body-state function.
    bodyCenteredPseudoBodyFixedStateFunction_ = std::bind(
                &reference_frames::AerodynamicAngleCalculator::getCurrentAirspeedBasedBodyFixedState, aerodynamicAngleCalculator_ );

    // Check if given body shape is an oblate spheroid and set geodetic latitude function if so
    if( std::dynamic_pointer_cast< basic_astrodynamics::OblateSpheroidBodyShapeModel >( shapeModel ) != nullptr )
    {
        geodeticLatitudeFunction_ = std::bind(
                    &basic_astrodynamics::OblateSpheroidBodyShapeModel::getGeodeticLatitude,
                    std::dynamic_pointer_cast< basic_astrodynamics::OblateSpheroidBodyShapeModel >( shapeModel ),
                    std::placeholders::_1, 1.0E-4 );
    }
}

//! Function to update all flight conditions.
void FlightConditions::updateConditions( const double currentTime )
{
    if( !( currentTime == currentTime_ ) )
    {
        currentTime_ = currentTime;

        // Update aerodynamic angles (but not angles w.r.t. body-fixed frame).
        if( aerodynamicAngleCalculator_!= nullptr )
        {
            aerodynamicAngleCalculator_->update( currentTime, false );
        }

        // Calculate state of vehicle in global frame and corotating frame.
        currentBodyCenteredAirspeedBasedBodyFixedState_ = bodyCenteredPseudoBodyFixedStateFunction_( );
    }
}

//! Constructor, sets objects and functions from which relevant environment and state variables are retrieved.
AtmosphericFlightConditions::AtmosphericFlightConditions(
        const std::shared_ptr< aerodynamics::AtmosphereModel > atmosphereModel,
        const std::shared_ptr< basic_astrodynamics::BodyShapeModel > shapeModel,
        const std::shared_ptr< AerodynamicCoefficientInterface > aerodynamicCoefficientInterface,
        const std::shared_ptr< reference_frames::AerodynamicAngleCalculator > aerodynamicAngleCalculator,
        const std::function< double( const std::string& ) > controlSurfaceDeflectionFunction ):
    FlightConditions( shapeModel, aerodynamicAngleCalculator ),
    atmosphereModel_( atmosphereModel ),
    aerodynamicCoefficientInterface_( aerodynamicCoefficientInterface ),
    controlSurfaceDeflectionFunction_( controlSurfaceDeflectionFunction )
{
    // Check if atmosphere requires latitude and longitude update.
    if( std::dynamic_pointer_cast< aerodynamics::StandardAtmosphere >( atmosphereModel_ ) == nullptr )
    {
        updateLatitudeAndLongitudeForAtmosphere_ = 1;
    }
    else
    {
        updateLatitudeAndLongitudeForAtmosphere_ = 0;
    }
    isLatitudeAndLongitudeSet_ = 0;

    if( updateLatitudeAndLongitudeForAtmosphere_ && aerodynamicAngleCalculator_== nullptr )
    {
        throw std::runtime_error( "Error when making flight conditions, angles are to be updated, but no calculator is set" );
    }
}

//! Function to set custom dependency of aerodynamic coefficients
void AtmosphericFlightConditions::setAerodynamicCoefficientsIndependentVariableFunction(
        const AerodynamicCoefficientsIndependentVariables independentVariable,
        const std::function< double( ) > coefficientDependency )
{
    if( ( independentVariable == mach_number_dependent ) ||
            ( independentVariable == angle_of_attack_dependent ) ||
            ( independentVariable == angle_of_sideslip_dependent )||
            ( independentVariable == altitude_dependent ) )
    {
        throw std::runtime_error(
                    std::string( "Error when setting aerodynamic coefficient function dependency, value of parameter " ) +
                    std::to_string( independentVariable ) +
                    std::string(", will not  be used." ) );
    }
    else
    {
        customCoefficientDependencies_[ independentVariable ] = coefficientDependency;
    }
}

//! Function to update all flight conditions.
void AtmosphericFlightConditions::updateConditions( const double currentTime )
{
    if( !( currentTime == currentTime_ ) )
    {
        currentTime_ = currentTime;

        // Update aerodynamic angles (but not angles w.r.t. body-fixed frame).
        if( aerodynamicAngleCalculator_!= nullptr )
        {
            aerodynamicAngleCalculator_->update( currentTime, false );
        }

        // Calculate state of vehicle in global frame and corotating frame.
        currentBodyCenteredAirspeedBasedBodyFixedState_ = bodyCenteredPseudoBodyFixedStateFunction_( );

        updateAerodynamicCoefficientInput( );

        // Update angles from aerodynamic to body-fixed frame (if relevant).
        if( aerodynamicAngleCalculator_!= nullptr )
        {
            aerodynamicAngleCalculator_->update( currentTime, true );
            updateAerodynamicCoefficientInput( );
        }

        // Update aerodynamic coefficients.
        aerodynamicCoefficientInterface_->updateFullCurrentCoefficients(
                    aerodynamicCoefficientIndependentVariables_, controlSurfaceAerodynamicCoefficientIndependentVariables_,
                    currentTime_ );
    }
}

//! Function to (compute and) retrieve the value of an independent variable of aerodynamic coefficients
double AtmosphericFlightConditions::getAerodynamicCoefficientIndependentVariable(
        const AerodynamicCoefficientsIndependentVariables independentVariableType,
        const std::string& secondaryIdentifier )
{
    double currentIndependentVariable;
    switch( independentVariableType )
    {
    //Calculate Mach number if needed.
    case mach_number_dependent:
        currentIndependentVariable = getCurrentMachNumber( );
        break;
        //Get angle of attack if needed.
    case angle_of_attack_dependent:
        if( aerodynamicAngleCalculator_== nullptr )
        {
            throw std::runtime_error( "Error, aerodynamic angle calculator is nullptr, but require angle of attack" );
        }
        currentIndependentVariable = aerodynamicAngleCalculator_->getAerodynamicAngle(
                    reference_frames::angle_of_attack );
        break;
        //Get angle of sideslip if needed.
    case angle_of_sideslip_dependent:
        if( aerodynamicAngleCalculator_== nullptr )
        {
            throw std::runtime_error( "Error, aerodynamic angle calculator is nullptr, but require angle of sideslip" );
        }
        currentIndependentVariable = aerodynamicAngleCalculator_->getAerodynamicAngle(
                    reference_frames::angle_of_sideslip );
        break;
    case altitude_dependent:
        currentIndependentVariable = getCurrentAltitude( );
        break;
    case control_surface_deflection_dependent:
    {
        try
        {
            currentIndependentVariable = controlSurfaceDeflectionFunction_( secondaryIdentifier );
        }
        catch( std::runtime_error )
        {
            throw std::runtime_error( "Error, control surface " + secondaryIdentifier + "not recognized when updating coefficients" );
        }
        break;
    }
    default:
        if( customCoefficientDependencies_.count( independentVariableType ) == 0 )
        {
            throw std::runtime_error( "Error, did not recognize aerodynamic coefficient dependency "
                                      + std::to_string( independentVariableType ) );
        }
        else
        {
            currentIndependentVariable = customCoefficientDependencies_.at( independentVariableType )( );
        }
    }

    return currentIndependentVariable;
}

//! Function to update the independent variables of the aerodynamic coefficient interface
void AtmosphericFlightConditions::updateAerodynamicCoefficientInput( )
{
    aerodynamicCoefficientIndependentVariables_.clear( );
    // Calculate independent variables for aerodynamic coefficients.
    for( unsigned int i = 0; i < aerodynamicCoefficientInterface_->getNumberOfIndependentVariables( ); i++ )
    {
        aerodynamicCoefficientIndependentVariables_.push_back(
                    getAerodynamicCoefficientIndependentVariable(
                        aerodynamicCoefficientInterface_->getIndependentVariableName( i ) ) );
    }

    controlSurfaceAerodynamicCoefficientIndependentVariables_.clear( );
    for( unsigned int i = 0; i < aerodynamicCoefficientInterface_->getNumberOfControlSurfaces( ); i++ )
    {
        std::string currentControlSurface = aerodynamicCoefficientInterface_->getControlSurfaceName( i );
        for( unsigned int j = 0; j < aerodynamicCoefficientInterface_->getNumberOfControlSurfaceIndependentVariables( currentControlSurface ); j++ )
        {
            controlSurfaceAerodynamicCoefficientIndependentVariables_[ currentControlSurface ].push_back(
                        getAerodynamicCoefficientIndependentVariable(
                            aerodynamicCoefficientInterface_->getControlSurfaceIndependentVariableName(
                                currentControlSurface, j ), currentControlSurface ) );
        }
    }
}

//! Function to set the angle of attack to trimmed conditions.
std::shared_ptr< TrimOrientationCalculator > setTrimmedConditions(
        const std::shared_ptr< AtmosphericFlightConditions > flightConditions )
{
    // Create trim object.
    std::shared_ptr< TrimOrientationCalculator > trimOrientation =
            std::make_shared< TrimOrientationCalculator >(
                flightConditions->getAerodynamicCoefficientInterface( ) );

    // Create angle-of-attack function from trim object.
    std::function< std::vector< double >( ) > untrimmedIndependentVariablesFunction =
            std::bind( &AtmosphericFlightConditions::getAerodynamicCoefficientIndependentVariables,
                         flightConditions );
    std::function< std::map< std::string, std::vector< double > >( ) > untrimmedControlSurfaceIndependentVariablesFunction =
            std::bind( &AtmosphericFlightConditions::getControlSurfaceAerodynamicCoefficientIndependentVariables,
                         flightConditions );
    flightConditions->getAerodynamicAngleCalculator( )->setOrientationAngleFunctions(
                std::bind( &TrimOrientationCalculator::findTrimAngleOfAttackFromFunction, trimOrientation,
                             untrimmedIndependentVariablesFunction, untrimmedControlSurfaceIndependentVariablesFunction ) );

    return trimOrientation;
}

} // namespace aerodynamics

} // namespace tudat