/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * 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.
 */

/*
 * Author: Stuart Glaser
 */

#include <boost/shared_ptr.hpp>

#include "robot_mechanism_controllers/joint_spline_trajectory_controller.h"
#include <sstream>
#include "angles/angles.h"
#include "pluginlib/class_list_macros.h"

PLUGINLIB_EXPORT_CLASS( controller::JointSplineTrajectoryController, pr2_controller_interface::Controller)

namespace controller {

// These functions are pulled from the spline_smoother package.
// They've been moved here to avoid depending on packages that aren't
// mature yet.

static inline void generatePowers(int n, double x, double* powers)
{
  powers[0] = 1.0;
  for (int i=1; i<=n; i++)
  {
    powers[i] = powers[i-1]*x;
  }
}

static void getQuinticSplineCoefficients(double start_pos, double start_vel, double start_acc,
    double end_pos, double end_vel, double end_acc, double time, std::vector<double>& coefficients)
{
  coefficients.resize(6);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.5*end_acc;
    coefficients[3] = 0.0;
    coefficients[4] = 0.0;
    coefficients[5] = 0.0;
  }
  else
  {
    double T[6];
    generatePowers(5, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = 0.5*start_acc;
    coefficients[3] = (-20.0*start_pos + 20.0*end_pos - 3.0*start_acc*T[2] + end_acc*T[2] -
                       12.0*start_vel*T[1] - 8.0*end_vel*T[1]) / (2.0*T[3]);
    coefficients[4] = (30.0*start_pos - 30.0*end_pos + 3.0*start_acc*T[2] - 2.0*end_acc*T[2] +
                       16.0*start_vel*T[1] + 14.0*end_vel*T[1]) / (2.0*T[4]);
    coefficients[5] = (-12.0*start_pos + 12.0*end_pos - start_acc*T[2] + end_acc*T[2] -
                       6.0*start_vel*T[1] - 6.0*end_vel*T[1]) / (2.0*T[5]);
  }
}

/**
 * \brief Samples a quintic spline segment at a particular time
 */
static void sampleQuinticSpline(const std::vector<double>& coefficients, double time,
    double& position, double& velocity, double& acceleration)
{
  // create powers of time:
  double t[6];
  generatePowers(5, time, t);

  position = t[0]*coefficients[0] +
      t[1]*coefficients[1] +
      t[2]*coefficients[2] +
      t[3]*coefficients[3] +
      t[4]*coefficients[4] +
      t[5]*coefficients[5];

  velocity = t[0]*coefficients[1] +
      2.0*t[1]*coefficients[2] +
      3.0*t[2]*coefficients[3] +
      4.0*t[3]*coefficients[4] +
      5.0*t[4]*coefficients[5];

  acceleration = 2.0*t[0]*coefficients[2] +
      6.0*t[1]*coefficients[3] +
      12.0*t[2]*coefficients[4] +
      20.0*t[3]*coefficients[5];
}

static void getCubicSplineCoefficients(double start_pos, double start_vel,
    double end_pos, double end_vel, double time, std::vector<double>& coefficients)
{
  coefficients.resize(4);

  if (time == 0.0)
  {
    coefficients[0] = end_pos;
    coefficients[1] = end_vel;
    coefficients[2] = 0.0;
    coefficients[3] = 0.0;
  }
  else
  {
    double T[4];
    generatePowers(3, time, T);

    coefficients[0] = start_pos;
    coefficients[1] = start_vel;
    coefficients[2] = (-3.0*start_pos + 3.0*end_pos - 2.0*start_vel*T[1] - end_vel*T[1]) / T[2];
    coefficients[3] = (2.0*start_pos - 2.0*end_pos + start_vel*T[1] + end_vel*T[1]) / T[3];
  }
}

JointSplineTrajectoryController::JointSplineTrajectoryController()
  : loop_count_(0), robot_(NULL)
{
}

JointSplineTrajectoryController::~JointSplineTrajectoryController()
{
  sub_command_.shutdown();
  serve_query_state_.shutdown();
}

bool JointSplineTrajectoryController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
{
  using namespace XmlRpc;
  node_ = n;
  robot_ = robot;

  // Gets all of the joints
  XmlRpc::XmlRpcValue joint_names;
  if (!node_.getParam("joints", joint_names))
  {
    ROS_ERROR("No joints given. (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Malformed joint specification.  (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  for (int i = 0; i < joint_names.size(); ++i)
  {
    XmlRpcValue &name_value = joint_names[i];
    if (name_value.getType() != XmlRpcValue::TypeString)
    {
      ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
                node_.getNamespace().c_str());
      return false;
    }

    pr2_mechanism_model::JointState *j = robot->getJointState((std::string)name_value);
    if (!j) {
      ROS_ERROR("Joint not found: %s. (namespace: %s)",
                ((std::string)name_value).c_str(), node_.getNamespace().c_str());
      return false;
    }
    joints_.push_back(j);
  }

  // Ensures that all the joints are calibrated.
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    if (!joints_[i]->calibrated_)
    {
      ROS_ERROR("Joint %s was not calibrated (namespace: %s)",
                joints_[i]->joint_->name.c_str(), node_.getNamespace().c_str());
      return false;
    }
  }

  // Sets up pid controllers for all of the joints
  std::string gains_ns;
  if (!node_.getParam("gains", gains_ns))
    gains_ns = node_.getNamespace() + "/gains";
  pids_.resize(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
    if (!pids_[i].init(ros::NodeHandle(gains_ns + "/" + joints_[i]->joint_->name)))
      return false;

  // Creates a dummy trajectory
  boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &traj = *traj_ptr;
  traj[0].start_time = robot_->getTime().toSec();
  traj[0].duration = 0.0;
  traj[0].splines.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    traj[0].splines[j].coef[0] = 0.0;
  current_trajectory_box_.set(traj_ptr);

  sub_command_ = node_.subscribe("command", 1, &JointSplineTrajectoryController::commandCB, this);
  serve_query_state_ = node_.advertiseService(
    "query_state", &JointSplineTrajectoryController::queryStateService, this);

  q.resize(joints_.size());
  qd.resize(joints_.size());
  qdd.resize(joints_.size());

  controller_state_publisher_.reset(
    new realtime_tools::RealtimePublisher<pr2_controllers_msgs::JointTrajectoryControllerState>
    (node_, "state", 1));
  controller_state_publisher_->lock();
  for (size_t j = 0; j < joints_.size(); ++j)
    controller_state_publisher_->msg_.joint_names.push_back(joints_[j]->joint_->name);
  controller_state_publisher_->msg_.desired.positions.resize(joints_.size());
  controller_state_publisher_->msg_.desired.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size());
  controller_state_publisher_->msg_.actual.positions.resize(joints_.size());
  controller_state_publisher_->msg_.actual.velocities.resize(joints_.size());
  controller_state_publisher_->msg_.error.positions.resize(joints_.size());
  controller_state_publisher_->msg_.error.velocities.resize(joints_.size());
  controller_state_publisher_->unlock();

  return true;
}

void JointSplineTrajectoryController::starting()
{
  last_time_ = robot_->getTime();

  for (size_t i = 0; i < pids_.size(); ++i)
    pids_[i].reset();

  // Creates a "hold current position" trajectory.
  boost::shared_ptr<SpecifiedTrajectory> hold_ptr(new SpecifiedTrajectory(1));
  SpecifiedTrajectory &hold = *hold_ptr;
  hold[0].start_time = last_time_.toSec() - 0.001;
  hold[0].duration = 0.0;
  hold[0].splines.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
    hold[0].splines[j].coef[0] = joints_[j]->position_;

  current_trajectory_box_.set(hold_ptr);
}

void JointSplineTrajectoryController::update()
{
  // Checks if all the joints are calibrated.

  ros::Time time = robot_->getTime();
  ros::Duration dt = time - last_time_;
  last_time_ = time;

  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.get(traj_ptr);
  if (!traj_ptr)
    ROS_FATAL("The current trajectory can never be null");

  // Only because this is what the code originally looked like.
  const SpecifiedTrajectory &traj = *traj_ptr;

  // Determines which segment of the trajectory to use.  (Not particularly realtime friendly).
  int seg = -1;
  while (seg + 1 < (int)traj.size() &&
         traj[seg+1].start_time < time.toSec())
  {
    ++seg;
  }

  if (seg == -1)
  {
    if (traj.size() == 0)
      ROS_ERROR("No segments in the trajectory");
    else
      ROS_ERROR("No earlier segments.  First segment starts at %.3lf (now = %.3lf)", traj[0].start_time, time.toSec());
    return;
  }

  // ------ Trajectory Sampling

  for (size_t i = 0; i < q.size(); ++i)
  {
    sampleSplineWithTimeBounds(traj[seg].splines[i].coef, traj[seg].duration,
                               time.toSec() - traj[seg].start_time,
                               q[i], qd[i], qdd[i]);
  }

  // ------ Trajectory Following

  std::vector<double> error(joints_.size());
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    error[i] = q[i] - joints_[i]->position_;
    joints_[i]->commanded_effort_ += pids_[i].computeCommand(error[i],
          joints_[i]->velocity_ - qd[i], dt);
  }

  // ------ State publishing

  if (loop_count_ % 10 == 0)
  {
    if (controller_state_publisher_ && controller_state_publisher_->trylock())
    {
      controller_state_publisher_->msg_.header.stamp = time;
      for (size_t j = 0; j < joints_.size(); ++j)
      {
        controller_state_publisher_->msg_.desired.positions[j] = q[j];
        controller_state_publisher_->msg_.desired.velocities[j] = qd[j];
        controller_state_publisher_->msg_.desired.accelerations[j] = qdd[j];
        controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_;
        controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_;
        controller_state_publisher_->msg_.error.positions[j] = error[j];
        controller_state_publisher_->msg_.error.velocities[j] = joints_[j]->velocity_ - qd[j];
      }
      controller_state_publisher_->unlockAndPublish();
    }
  }

  ++loop_count_;
}

void JointSplineTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
  ros::Time time = last_time_;
  ROS_DEBUG("Figuring out new trajectory at %.3lf, with data from %.3lf",
            time.toSec(), msg->header.stamp.toSec());

  boost::shared_ptr<SpecifiedTrajectory> new_traj_ptr(new SpecifiedTrajectory);
  SpecifiedTrajectory &new_traj = *new_traj_ptr;

  // ------ If requested, performs a stop

  if (msg->points.empty())
  {
    starting();
    return;
  }

  // ------ Correlates the joints we're commanding to the joints in the message

  std::vector<int> lookup(joints_.size(), -1);  // Maps from an index in joints_ to an index in the msg
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    for (size_t k = 0; k < msg->joint_names.size(); ++k)
    {
      if (msg->joint_names[k] == joints_[j]->joint_->name)
      {
        lookup[j] = k;
        break;
      }
    }

    if (lookup[j] == -1)
    {
      ROS_ERROR("Unable to locate joint %s in the commanded trajectory.", joints_[j]->joint_->name.c_str());
      return;
    }
  }

  // ------ Grabs the trajectory that we're currently following.

  boost::shared_ptr<const SpecifiedTrajectory> prev_traj_ptr;
  current_trajectory_box_.get(prev_traj_ptr);
  if (!prev_traj_ptr)
  {
    ROS_FATAL("The current trajectory can never be null");
    return;
  }
  const SpecifiedTrajectory &prev_traj = *prev_traj_ptr;

  // ------ Copies over the segments from the previous trajectory that are still useful.

  // Useful segments are still relevant after the current time.
  int first_useful = -1;
  while (first_useful + 1 < (int)prev_traj.size() &&
         prev_traj[first_useful + 1].start_time <= time.toSec())
  {
    ++first_useful;
  }

  // Useful segments are not going to be completely overwritten by the message's splines.
  int last_useful = -1;
  double msg_start_time;
  if (msg->points.size() > 0)
    msg_start_time = (msg->header.stamp + msg->points[0].time_from_start).toSec();
  else
    msg_start_time = std::max(time.toSec(), msg->header.stamp.toSec());

  while (last_useful + 1 < (int)prev_traj.size() &&
         prev_traj[last_useful + 1].start_time < msg_start_time)
  {
    ++last_useful;
  }

  if (last_useful < first_useful)
    first_useful = last_useful;

  // Copies over the old segments that were determined to be useful.
  for (int i = std::max(first_useful,0); i <= last_useful; ++i)
  {
    new_traj.push_back(prev_traj[i]);
  }

  // We always save the last segment so that we know where to stop if
  // there are no new segments.
  if (new_traj.size() == 0)
    new_traj.push_back(prev_traj[prev_traj.size() - 1]);

  // ------ Determines when and where the new segments start

  // Finds the end conditions of the final segment
  Segment &last = new_traj[new_traj.size() - 1];
  std::vector<double> prev_positions(joints_.size());
  std::vector<double> prev_velocities(joints_.size());
  std::vector<double> prev_accelerations(joints_.size());

  ROS_DEBUG("Initial conditions for new set of splines:");
  for (size_t i = 0; i < joints_.size(); ++i)
  {
    sampleSplineWithTimeBounds(last.splines[i].coef, last.duration,
                               msg->header.stamp.toSec() - last.start_time,
                               prev_positions[i], prev_velocities[i], prev_accelerations[i]);
    ROS_DEBUG("    %.2lf, %.2lf, %.2lf  (%s)", prev_positions[i], prev_velocities[i],
              prev_accelerations[i], joints_[i]->joint_->name.c_str());
  }

  // ------ Tacks on the new segments

  std::vector<double> positions;
  std::vector<double> velocities;
  std::vector<double> accelerations;

  std::vector<double> durations(msg->points.size());
  durations[0] = msg->points[0].time_from_start.toSec();
  for (size_t i = 1; i < msg->points.size(); ++i)
    durations[i] = (msg->points[i].time_from_start - msg->points[i-1].time_from_start).toSec();

  // Checks if we should wrap
  std::vector<double> wrap(joints_.size(), 0.0);
  assert(!msg->points[0].positions.empty());
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    if (joints_[j]->joint_->type == urdf::Joint::CONTINUOUS)
    {
      double dist = angles::shortest_angular_distance(prev_positions[j], msg->points[0].positions[j]);
      wrap[j] = (prev_positions[j] + dist) - msg->points[0].positions[j];
    }
  }

  for (size_t i = 0; i < msg->points.size(); ++i)
  {
    Segment seg;

    seg.start_time = (msg->header.stamp + msg->points[i].time_from_start).toSec() - durations[i];
    seg.duration = durations[i];
    seg.splines.resize(joints_.size());

    // Checks that the incoming segment has the right number of elements.

    if (msg->points[i].accelerations.size() != 0 && msg->points[i].accelerations.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the accelerations", (int)i, (int)msg->points[i].accelerations.size());
      return;
    }
    if (msg->points[i].velocities.size() != 0 && msg->points[i].velocities.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the velocities", (int)i, (int)msg->points[i].velocities.size());
      return;
    }
    if (msg->points[i].positions.size() != joints_.size())
    {
      ROS_ERROR("Command point %d has %d elements for the positions", (int)i, (int)msg->points[i].positions.size());
      return;
    }

    // Re-orders the joints in the command to match the interal joint order.

    accelerations.resize(msg->points[i].accelerations.size());
    velocities.resize(msg->points[i].velocities.size());
    positions.resize(msg->points[i].positions.size());
    for (size_t j = 0; j < joints_.size(); ++j)
    {
      if (!accelerations.empty()) accelerations[j] = msg->points[i].accelerations[lookup[j]];
      if (!velocities.empty()) velocities[j] = msg->points[i].velocities[lookup[j]];
      if (!positions.empty()) positions[j] = msg->points[i].positions[lookup[j]] + wrap[j];
    }

    // Converts the boundary conditions to splines.

    for (size_t j = 0; j < joints_.size(); ++j)
    {
      if (prev_accelerations.size() > 0 && accelerations.size() > 0)
      {
        getQuinticSplineCoefficients(
          prev_positions[j], prev_velocities[j], prev_accelerations[j],
          positions[j], velocities[j], accelerations[j],
          durations[i],
          seg.splines[j].coef);
      }
      else if (prev_velocities.size() > 0 && velocities.size() > 0)
      {
        getCubicSplineCoefficients(
          prev_positions[j], prev_velocities[j],
          positions[j], velocities[j],
          durations[i],
          seg.splines[j].coef);
        seg.splines[j].coef.resize(6, 0.0);
      }
      else
      {
        seg.splines[j].coef[0] = prev_positions[j];
        if (durations[i] == 0.0)
          seg.splines[j].coef[1] = 0.0;
        else
          seg.splines[j].coef[1] = (positions[j] - prev_positions[j]) / durations[i];
        seg.splines[j].coef[2] = 0.0;
        seg.splines[j].coef[3] = 0.0;
        seg.splines[j].coef[4] = 0.0;
        seg.splines[j].coef[5] = 0.0;
      }
    }

    // Pushes the splines onto the end of the new trajectory.

    new_traj.push_back(seg);

    // Computes the starting conditions for the next segment

    prev_positions = positions;
    prev_velocities = velocities;
    prev_accelerations = accelerations;
  }

  // ------ Commits the new trajectory

  if (!new_traj_ptr)
  {
    ROS_ERROR("The new trajectory was null!");
    return;
  }

  current_trajectory_box_.set(new_traj_ptr);
  ROS_DEBUG("The new trajectory has %d segments", (int)new_traj.size());
#if 0
  for (size_t i = 0; i < std::min((size_t)20,new_traj.size()); ++i)
  {
    ROS_DEBUG("Segment %2d: %.3lf for %.3lf", i, new_traj[i].start_time, new_traj[i].duration);
    for (size_t j = 0; j < new_traj[i].splines.size(); ++j)
    {
      ROS_DEBUG("    %.2lf  %.2lf  %.2lf  %.2lf , %.2lf  %.2lf(%s)",
                new_traj[i].splines[j].coef[0],
                new_traj[i].splines[j].coef[1],
                new_traj[i].splines[j].coef[2],
                new_traj[i].splines[j].coef[3],
                new_traj[i].splines[j].coef[4],
                new_traj[i].splines[j].coef[5],
                joints_[j]->joint_->name_.c_str());
    }
  }
#endif
}

bool JointSplineTrajectoryController::queryStateService(
  pr2_controllers_msgs::QueryTrajectoryState::Request &req,
  pr2_controllers_msgs::QueryTrajectoryState::Response &resp)
{
  boost::shared_ptr<const SpecifiedTrajectory> traj_ptr;
  current_trajectory_box_.get(traj_ptr);
  if (!traj_ptr)
  {
    ROS_FATAL("The current trajectory can never be null");
    return false;
  }
  const SpecifiedTrajectory &traj = *traj_ptr;

  // Determines which segment of the trajectory to use
  int seg = -1;
  while (seg + 1 < (int)traj.size() &&
         traj[seg+1].start_time < req.time.toSec())
  {
    ++seg;
  }
  if (seg == -1)
    return false;

  for (size_t i = 0; i < q.size(); ++i)
  {
  }

  resp.name.resize(joints_.size());
  resp.position.resize(joints_.size());
  resp.velocity.resize(joints_.size());
  resp.acceleration.resize(joints_.size());
  for (size_t j = 0; j < joints_.size(); ++j)
  {
    resp.name[j] = joints_[j]->joint_->name;
    sampleSplineWithTimeBounds(traj[seg].splines[j].coef, traj[seg].duration,
                               req.time.toSec() - traj[seg].start_time,
                               resp.position[j], resp.velocity[j], resp.acceleration[j]);
  }

  return true;
}

void JointSplineTrajectoryController::sampleSplineWithTimeBounds(
  const std::vector<double>& coefficients, double duration, double time,
  double& position, double& velocity, double& acceleration)
{
  if (time < 0)
  {
    double _;
    sampleQuinticSpline(coefficients, 0.0, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else if (time > duration)
  {
    double _;
    sampleQuinticSpline(coefficients, duration, position, _, _);
    velocity = 0;
    acceleration = 0;
  }
  else
  {
    sampleQuinticSpline(coefficients, time,
                        position, velocity, acceleration);
  }
}

}


/**
 * @file fusion.hpp
 * @brief The main header file for the Code Base
 *
 */

#ifndef _FUSION_HPP_
#define _FUSION_HPP_

//INITIAL STATE ENTER BEFORE FLIGHT
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

#define x_x_0 0 //initial position in x direction
#define x_y_0 0 //initial position in y direction
#define x_z_0 0 //initial position in z direction
#define v_x_0 0 // initial velocity in the x direction
#define v_y_0 0 // initial velocity in the y direction
#define v_z_0 0 // initial velocity in the z direction
#define a_x_0 0 // initial acceleration in x direction
#define a_y_0 0 // initial acceleration in y direction
#define a_z_0 0 // initial acceleration in z direction
#define theta_x_0 0 // initial attitude in the x direction
#define theta_y_0 0 // initial attitude in the y direction
#define theta_z_0 0 // initial attitude in the z direction
#define omega_x_0 0 // initial angular velocity in the x direction
#define omega_y_0 0 // initial angular velocity in the y direction
#define omega_z_0 0 // initial angular velocity in the z direction
#define alpha_x_0 0 // initial angular acceleration in the x direction
#define alpha_y_0 0 // initial angular acceleration in the y direction
#define alpha_z_0 0 // initial angular acceleration in the z direction
#define mag_x_0 0 //  nitial expected magnetic field reading in the x direction
#define mag_y_0 0 // initial expected magnetic field reading in the y direction
#define mag_z_0 0 // initial expected magnetic field reading in the z direction

#define p_x_x_0 0.1 //initial position variance in x direction
#define p_x_y_0 0.1 //initial position variance in y direction
#define p_x_z_0 0.1 //initial position variance in z direction
#define p_v_x_0 0.1 // initial velocity variance in the x direction
#define p_v_y_0 0.1 // initial velocity variance in the y direction
#define p_v_z_0 0.1 // initial velocity variance in the z direction
#define p_a_x_0 0.01 // initial acceleration variance in x direction
#define p_a_y_0 0.01 // initial acceleration variance in y direction
#define p_a_z_0 0.01 // initial acceleration variance in z direction
#define p_theta_x_0 0.1 // initial attitude variance in the x direction
#define p_theta_y_0 0.1 // initial attitude variance in the y direction
#define p_theta_z_0 0.1 // initial attitude variance in the z direction
#define p_omega_x_0 0.1 // initial angular velocity variance in the x direction
#define p_omega_y_0 0.1 // initial angular velocity variance in the y direction
#define p_omega_z_0 0.1 // initial angular velocity variance in the z direction
#define p_alpha_x_0 0.01 // initial angular acceleration variance in the x direction
#define p_alpha_y_0 0.01 // initial angular acceleration variance in the y direction
#define p_alpha_z_0 0.01 // initial angular acceleration variance in the z direction
#define p_mag_x_0 0 // initial expected magnetic field reading variance in the x direction
#define p_mag_y_0 0 // initial expected magnetic field reading variance in the y direction
#define p_mag_z_0 0 // initial expected magnetic field reading variance in the z direction

///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////

#include "Arduino.h"
#include "Eigen.h"
#include <Eigen/Eigen>
#include <Adafruit_BNO055.h>
#include "Thread.h"
#include <ThreadController.h>

//! IMU Struct
/*!
*   This structs holds the BNO055 sample at a point in time to be stored and processed.
*/
struct IMUdata {
    float GYRO[3] = {0,0,0};             // Gyro in [DPS]
    float LINEAR_ACCEL[3] = {0,0,0};     // Accelerometer in [m/s^2]
    float GRAVITY_ACCEL[3] = {0,0,0};    // Accelerometer in [mg]
    float MAG[3] = {0,0,0};              // Magnetometer in [uT]
    float Quat[4] = {0,0,0,0};           // {w, x, y, z}
    float Temp = 0;
    uint32_t t = 0;

    float phi, theta, psi;                // Roll or phi (X), pitch or theta (Y), and yaw or psi (Z)
    float q_w, q_x, q_y, q_z;             // Quaternion
};

//! Main State Estimation Class
/*!
*
*/
class State
{
    private:
        float accel_error = 0.0;

        float gyro_error = 0.0;
        IMUdata *data;

    public:
        State(IMUdata*);
        ~State();

        void dataAq(IMUdata *data);

        void predict();
        void processCovarianceMatrix();
        void calculateKalmanGain();
        void stateDetermination();
        void updatePreviousState();
        void updateProcessCovarianceMatrix();
        void updateDynamics();
        // Eigen::MatrixXd dcmBodyToEarth(double theta, double phi, double psi);

        template<typename T>
        void print_mtxd(const T& X);

        float calcAccelSystematicError();
        float calcGyroSystematicError();

        float eulerAngle(float GYRO, float gyro_sen, float gyro_samp); // Function to compute Euler angle
        void quaternion(float phi, float theta, float psi, float &q_w, float &q_x, float &q_y, float &q_z); // Function to computer Quaternion

    protected:
};

//! BNO055 IMU Class
/*!
*   Class to manage the Adafruit BNO055 Absolute Orientation IMU Fusion breakout board
*/
class DigitalIMU {
    private:
        Adafruit_BNO055 board;
        sensors_event_t event;
        imu::Quaternion quat;
        imu::Vector<3> accel;
    public:
        DigitalIMU();
        DigitalIMU(int32_t sensorID, uint8_t address);
        bool begin();
        void sample(IMUdata* data, State* ptr);
};

namespace constants
{
    extern int interval_IMU;
    extern double dt;
    extern double baseAccel_error;
    extern double baseGyro_error;
    extern float gyro_sen;           // = 900 rad/sec
};

/* All lower case variables are vectors || all uppercase are Matrices*/
namespace matrices
{
    // BOTH DISCARDED AFTER FIRST RUN
    extern Eigen::VectorXd x_0; // Initial State Vector
    extern Eigen::MatrixXd P_0; // Initial Process Covariance Matrix

    // VECTORS
    extern Eigen::VectorXd y; /* Observation Vector */
    extern Eigen::VectorXd x_m; /* Direct Observation Vector */
    extern Eigen::VectorXd x_k; /* Final State Vector */
    extern Eigen::VectorXd x_kp; /* State Prediction */
    extern Eigen::VectorXd x_k_1; /* Previous State Vector (K-1) */
    extern Eigen::VectorXd w_k; /* Predicted State Noise */
    extern Eigen::VectorXd z_k; /* Measurement Noise */

    // MATRICES
    extern Eigen::MatrixXd C; /* Observation Transition Matrix*/
    extern Eigen::MatrixXd A; /* State Transition Matrix */
    extern Eigen::MatrixXd P_kp; /* Predicted Process Covariance */
    extern Eigen::MatrixXd P_k_1; /* Previous Predicted Process Covariance */
    extern Eigen::MatrixXd R; /* Sensor Covariance Matrix */
    extern Eigen::MatrixXd H; /* Observation model mapping matrix*/
    extern Eigen::MatrixXd K; /* Kalman Gain */

    // Identity Matrix
    extern Eigen::MatrixXd I; /* 21x21 Identity Matrix */
 };

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-hyperbolic
    Function object implementing sinhcosh capabilities

    Computes simultaneously (and more rapidly) the sinh and cosh of the input

    @par Semantic:

    @code
    T ch, sh
    sinhcosh(x, sh, ch);
    @endcode

    is similar to:

    @code
    T sh = sinh(x);
    T ch = cosh(x);
    @endcode

    @see  sinh, cosh

  **/
  const boost::dispatch::functor<tag::sinhcosh_> sinhcosh = {};
} }
#endif

#include <boost/simd/function/scalar/sinhcosh.hpp>
#include <boost/simd/function/simd/sinhcosh.hpp>

#endif


#include "liver/macrocell_tree.hpp"
#include "liver/print_tree_stats.hpp"
#include "utility/math.hpp"
#include "utility/options.hpp"
#include "utility/volume_image.hpp"
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

using namespace jhmi;

int main(int argc, char* argv[]) {
  GOOGLE_PROTOBUF_VERIFY_VERSION;

  bool save_tree = false;
  auto opts = options<treefile_option, data_directory_option>{};
  opts.description().add_options()
      ("print-tree,p", boost::program_options::bool_switch(&save_tree));
  if (!opts.parse(argc, argv))
    return 1;

  auto liver = voxelized_shape{opts.data_directory() / "liver_extents.datz"};
  auto tree = macrocell_tree{load_tree, opts.treefile(), liver};
  auto terminal_nodes = tree.vessel_tree().terminal_vessel_nodes() | ranges::to_vector;
  auto get_avg_radius = [&] { return mean(terminal_nodes | ranges::view::transform([](auto n) {
        return n.value().radius(); })); };

  auto avg_goal = 11.8_um / 2.;
  auto avg_actual = get_avg_radius();
  fmt::print("Average radius desired: {}\n", avg_goal);
  fmt::print("Average radius actual: {}\n", avg_actual);

  while (abs(avg_actual - avg_goal) > 1_um) {
    tree.scale_terminal_vessels(avg_goal / avg_actual);

    avg_actual = get_avg_radius();
    fmt::print("Average radius actual: {}\n", avg_actual);
  }
  //tree.validate();
  print_tree_stats(tree, liver);

  if (save_tree)
    tree.write(opts.treefile().parent_path() / "vessel_tree.scaled.pbz");

  google::protobuf::ShutdownProtobufLibrary();
}


/* boost random/binomial_distribution.hpp header file
 *
 * Copyright Steven Watanabe 2010
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP_INCLUDED
#define BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP_INCLUDED

#include <boost/config/no_tr1/cmath.hpp>
#include <cstdlib>
#include <iosfwd>

#include <boost/random/detail/config.hpp>
#include <boost/random/uniform_01.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

namespace detail {

template<class RealType>
struct binomial_table {
    static const RealType table[10];
};

template<class RealType>
const RealType binomial_table<RealType>::table[10] = {
    0.08106146679532726,
    0.04134069595540929,
    0.02767792568499834,
    0.02079067210376509,
    0.01664469118982119,
    0.01387612882307075,
    0.01189670994589177,
    0.01041126526197209,
    0.009255462182712733,
    0.008330563433362871
};

}

/**
 * The binomial distribution is an integer valued distribution with
 * two parameters, @c t and @c p.  The values of the distribution
 * are within the range [0,t].
 *
 * The distribution function is
 * \f$\displaystyle P(k) = {t \choose k}p^k(1-p)^{t-k}\f$.
 *
 * The algorithm used is the BTRD algorithm described in
 *
 *  @blockquote
 *  "The generation of binomial random variates", Wolfgang Hormann,
 *  Journal of Statistical Computation and Simulation, Volume 46,
 *  Issue 1 & 2 April 1993 , pages 101 - 110
 *  @endblockquote
 */
template<class IntType = int, class RealType = double>
class binomial_distribution {
public:
    typedef IntType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef binomial_distribution distribution_type;
        /**
         * Construct a param_type object.  @c t and @c p
         * are the parameters of the distribution.
         *
         * Requires: t >=0 && 0 <= p <= 1
         */
        explicit param_type(IntType t_arg = 1, RealType p_arg = RealType (0.5))
          : _t(t_arg), _p(p_arg)
        {}
        /** Returns the @c t parameter of the distribution. */
        IntType t() const { return _t; }
        /** Returns the @c p parameter of the distribution. */
        RealType p() const { return _p; }
#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
        /** Writes the parameters of the distribution to a @c std::ostream. */
        template<class CharT, class Traits>
        friend std::basic_ostream<CharT,Traits>&
        operator<<(std::basic_ostream<CharT,Traits>& os,
                   const param_type& parm)
        {
            os << parm._p << " " << parm._t;
            return os;
        }

        /** Reads the parameters of the distribution from a @c std::istream. */
        template<class CharT, class Traits>
        friend std::basic_istream<CharT,Traits>&
        operator>>(std::basic_istream<CharT,Traits>& is, param_type& parm)
        {
            is >> parm._p >> std::ws >> parm._t;
            return is;
        }
#endif
        /** Returns true if the parameters have the same values. */
        friend bool operator==(const param_type& lhs, const param_type& rhs)
        {
            return lhs._t == rhs._t && lhs._p == rhs._p;
        }
        /** Returns true if the parameters have different values. */
        friend bool operator!=(const param_type& lhs, const param_type& rhs)
        {
            return !(lhs == rhs);
        }
    private:
        IntType _t;
        RealType _p;
    };

    /**
     * Construct a @c binomial_distribution object. @c t and @c p
     * are the parameters of the distribution.
     *
     * Requires: t >=0 && 0 <= p <= 1
     */
    explicit binomial_distribution(IntType t_arg = 1,
                                   RealType p_arg = RealType(0.5))
      : _t(t_arg), _p(p_arg)
    {
        init();
    }

    /**
     * Construct an @c binomial_distribution object from the
     * parameters.
     */
    explicit binomial_distribution(const param_type& parm)
      : _t(parm.t()), _p(parm.p())
    {
        init();
    }

    /**
     * Returns a random variate distributed according to the
     * binomial distribution.
     */
    template<class URNG>
    IntType operator()(URNG& urng) const
    {
        if(use_inversion()) {
            if(0.5 < _p) {
                return _t - invert(_t, 1-_p, urng);
            } else {
                return invert(_t, _p, urng);
            }
        } else if(0.5 < _p) {
            return _t - generate(urng);
        } else {
            return generate(urng);
        }
    }

    /**
     * Returns a random variate distributed according to the
     * binomial distribution with parameters specified by @c param.
     */
    template<class URNG>
    IntType operator()(URNG& urng, const param_type& parm) const
    {
        return binomial_distribution(parm)(urng);
    }

    /** Returns the @c t parameter of the distribution. */
    IntType t() const { return _t; }
    /** Returns the @c p parameter of the distribution. */
    RealType p() const { return _p; }

    /** Returns the smallest value that the distribution can produce. */
    IntType min BOOST_PREVENT_MACRO_SUBSTITUTION() const { return 0; }
    /** Returns the largest value that the distribution can produce. */
    IntType max BOOST_PREVENT_MACRO_SUBSTITUTION() const { return _t; }

    /** Returns the parameters of the distribution. */
    param_type param() const { return param_type(_t, _p); }
    /** Sets parameters of the distribution. */
    void param(const param_type& parm)
    {
        _t = parm.t();
        _p = parm.p();
        init();
    }

    /**
     * Effects: Subsequent uses of the distribution do not depend
     * on values produced by any engine prior to invoking reset.
     */
    void reset() { }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
    /** Writes the parameters of the distribution to a @c std::ostream. */
    template<class CharT, class Traits>
    friend std::basic_ostream<CharT,Traits>&
    operator<<(std::basic_ostream<CharT,Traits>& os,
               const binomial_distribution& bd)
    {
        os << bd.param();
        return os;
    }

    /** Reads the parameters of the distribution from a @c std::istream. */
    template<class CharT, class Traits>
    friend std::basic_istream<CharT,Traits>&
    operator>>(std::basic_istream<CharT,Traits>& is, binomial_distribution& bd)
    {
        bd.read(is);
        return is;
    }
#endif

    /** Returns true if the two distributions will produce the same
        sequence of values, given equal generators. */
    friend bool operator==(const binomial_distribution& lhs,
                           const binomial_distribution& rhs)
    {
        return lhs._t == rhs._t && lhs._p == rhs._p;
    }
    /** Returns true if the two distributions could produce different
        sequences of values, given equal generators. */
    friend bool operator!=(const binomial_distribution& lhs,
                           const binomial_distribution& rhs)
    {
        return !(lhs == rhs);
    }

private:

    /// @cond show_private

    template<class CharT, class Traits>
    void read(std::basic_istream<CharT, Traits>& is) {
        param_type parm;
        if(is >> parm) {
            param(parm);
        }
    }

    bool use_inversion() const
    {
        // BTRD is safe when np >= 10
        return m < 11;
    }

    // computes the correction factor for the Stirling approximation
    // for log(k!)
    static RealType fc(IntType k)
    {
        if(k < 10) return detail::binomial_table<RealType>::table[k];
        else {
            RealType ikp1 = RealType(1) / (k + 1);
            return (RealType(1)/12
                 - (RealType(1)/360
                 - (RealType(1)/1260)*(ikp1*ikp1))*(ikp1*ikp1))*ikp1;
        }
    }

    void init()
    {
        using std::sqrt;
        using std::pow;

        RealType p = (0.5 < _p)? (1 - _p) : _p;
        IntType t = _t;

        m = static_cast<IntType>((t+1)*p);

        if(use_inversion()) {
            _u.q_n = pow((1 - p), static_cast<RealType>(t));
        } else {
            _u.btrd.r = p/(1-p);
            _u.btrd.nr = (t+1)*_u.btrd.r;
            _u.btrd.npq = t*p*(1-p);
            RealType sqrt_npq = sqrt(_u.btrd.npq);
            _u.btrd.b = 1.15 + 2.53 * sqrt_npq;
            _u.btrd.a = -0.0873 + 0.0248*_u.btrd.b + 0.01*p;
            _u.btrd.c = t*p + 0.5;
            _u.btrd.alpha = (2.83 + 5.1/_u.btrd.b) * sqrt_npq;
            _u.btrd.v_r = 0.92 - 4.2/_u.btrd.b;
            _u.btrd.u_rv_r = 0.86*_u.btrd.v_r;
        }
    }

    template<class URNG>
    result_type generate(URNG& urng) const
    {
        using std::floor;
        using std::abs;
        using std::log;

        while(true) {
            RealType u;
            RealType v = uniform_01<RealType>()(urng);
            if(v <= _u.btrd.u_rv_r) {
                u = v/_u.btrd.v_r - 0.43;
                return static_cast<IntType>(floor(
                    (2*_u.btrd.a/(0.5 - abs(u)) + _u.btrd.b)*u + _u.btrd.c));
            }

            if(v >= _u.btrd.v_r) {
                u = uniform_01<RealType>()(urng) - 0.5;
            } else {
                u = v/_u.btrd.v_r - 0.93;
                u = ((u < 0)? -0.5 : 0.5) - u;
                v = uniform_01<RealType>()(urng) * _u.btrd.v_r;
            }

            RealType us = 0.5 - abs(u);
            IntType k = static_cast<IntType>(floor((2*_u.btrd.a/us + _u.btrd.b)*u + _u.btrd.c));
            if(k < 0 || k > _t) continue;
            v = v*_u.btrd.alpha/(_u.btrd.a/(us*us) + _u.btrd.b);
            RealType km = abs(k - m);
            if(km <= 15) {
                RealType f = 1;
                if(m < k) {
                    IntType i = m;
                    do {
                        ++i;
                        f = f*(_u.btrd.nr/i - _u.btrd.r);
                    } while(i != k);
                } else if(m > k) {
                    IntType i = k;
                    do {
                        ++i;
                        v = v*(_u.btrd.nr/i - _u.btrd.r);
                    } while(i != m);
                }
                if(v <= f) return k;
                else continue;
            } else {
                // final acceptance/rejection
                v = log(v);
                RealType rho =
                    (km/_u.btrd.npq)*(((km/3. + 0.625)*km + 1./6)/_u.btrd.npq + 0.5);
                RealType t = -km*km/(2*_u.btrd.npq);
                if(v < t - rho) return k;
                if(v > t + rho) continue;

                IntType nm = _t - m + 1;
                RealType h = (m + 0.5)*log((m + 1)/(_u.btrd.r*nm))
                           + fc(m) + fc(_t - m);

                IntType nk = _t - k + 1;
                if(v <= h + (_t+1)*log(static_cast<RealType>(nm)/nk)
                          + (k + 0.5)*log(nk*_u.btrd.r/(k+1))
                          - fc(k)
                          - fc(_t - k))
                {
                    return k;
                } else {
                    continue;
                }
            }
        }
    }

    template<class URNG>
    IntType invert(IntType t, RealType p, URNG& urng) const
    {
        RealType q = 1 - p;
        RealType s = p / q;
        RealType a = (t + 1) * s;
        RealType r = _u.q_n;
        RealType u = uniform_01<RealType>()(urng);
        IntType x = 0;
        while(u > r) {
            u = u - r;
            ++x;
            RealType r1 = ((a/x) - s) * r;
            // If r gets too small then the round-off error
            // becomes a problem.  At this point, p(i) is
            // decreasing exponentially, so if we just call
            // it 0, it's close enough.  Note that the
            // minimum value of q_n is about 1e-7, so we
            // may need to be a little careful to make sure that
            // we don't terminate the first time through the loop
            // for float.  (Hence the test that r is decreasing)
            if(r1 < std::numeric_limits<RealType>::epsilon() && r1 < r) {
                break;
            }
            r = r1;
        }
        return x;
    }

    // parameters
    IntType _t;
    RealType _p;

    // common data
    IntType m;

    union {
        // for btrd
        struct {
            RealType r;
            RealType nr;
            RealType npq;
            RealType b;
            RealType a;
            RealType c;
            RealType alpha;
            RealType v_r;
            RealType u_rv_r;
        } btrd;
        // for inversion
        RealType q_n;
    } _u;

    /// @endcond
};

}

// backwards compatibility
using random::binomial_distribution;

}

#include <boost/random/detail/enable_warnings.hpp>

#endif


/*
 * Copyright Nick Thompson, 2017
 * Use, modification and distribution are 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)
 */

#ifdef _MSC_VER
#pragma warning(disable:4459)
#endif

#if !defined(_MSC_VER) || (_MSC_VER >= 1900)

#include <boost/math/quadrature/naive_monte_carlo.hpp>
#include "test_compile_result.hpp"

using boost::math::quadrature::naive_monte_carlo;
void compile_and_link_test()
{
    auto g = [&](std::vector<double> const & x)
    {
        return 1.873;
    };
    std::vector<std::pair<double, double>> bounds{{0, 1}, {0, 1}, {0, 1}};
    naive_monte_carlo<double, decltype(g)> mc(g, bounds, 1.0);

    auto task = mc.integrate();
    check_result<double>(task.get());
}

#else
void compile_and_link_test()
{
}
#endif


#include "score.h"

#ifdef CMAKE_USE_DLIB
#include <dlib/matrix.h>
#endif

#include <algorithm>
#include <cassert>
#include <random>
#include <set>

namespace score
{

#ifdef CMAKE_USE_DLIB
const dat::Spectrum & find_worst( Comp c, const dat::Dataset & d )
{
    using P = std::pair< int, const dat::Spectrum & >;
    std::vector< P > scores;
    dat::apply( [ & ] ( int, const dat::Spectrum & s )
        {
            scores.emplace_back( c( s ), s );
        }
             , d );

    const auto & m = * std::max_element( scores.cbegin()
                                       , scores.cend()
                                       , [] ( const P & p1, const P & p2 )
        {
            int a = p1.first;
            int b = p2.first;
            return a > b;
            //return p1.first < p2.second;
        }    );

    return m.second;
}

std::set< label::Num > get_classes( const std::vector< label::Num > & v1
                                  , const std::vector< label::Num > & v2 )
{
    std::set< label::Num > all{ v1.cbegin(), v1.cend() };
    all.insert( v2.cbegin(), v2.cend() );
    return all;
}

struct Transcoder
{
    Transcoder( const std::set< label::Num > & classes )
    {
        int counter {};
        for( const auto c : classes )
        {
            _label_to_encoded[ c ] = counter++;
        }
    }

    int encode( label::Num label )
    {
        return _label_to_encoded[ label ];
    }

private:
    std::unordered_map<int, int> _label_to_encoded;
};

// Challange: some predicted labels may not exist in the ground_truth.
// Challange: labels can be arbitrary numbers.
dlib::matrix< unsigned >
calc_confusion( const std::vector< label::Num > &ground_truth
              , const std::vector< label::Num > &predicted )
{
    assert( ground_truth.size() == predicted.size() );

    const auto classes = get_classes( ground_truth, predicted );
    Transcoder t{ classes };
    const long c = static_cast<long>( classes.size() );
    dlib::matrix<unsigned> ret = dlib::zeros_matrix<unsigned>( c, c );

    for( unsigned i {}; i < ground_truth.size(); ++i )
    {
        const auto row = t.encode( ground_truth[ i ] );
        const auto col = t.encode( predicted[ i ] );
        ++ret( row, col );
    }

    return ret;
}

double accuracy( const Confusion & c )
{
    const auto t{ dlib::trace( c ) };
    const auto s{ dlib::sum( c ) };
    return 1.0 * t / s;
}
#endif  // CMAKE_USE_DLIB

}  // namespace score


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

#ifndef NETKET_ADADELTA_HPP
#define NETKET_ADADELTA_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class AdaDelta : public AbstractOptimizer {
  int npar_;

  double rho_;
  double epscut_;

  Eigen::VectorXd Eg2_;
  Eigen::VectorXd Edx2_;

  const Complex I_;

 public:
  // Json constructor
  explicit AdaDelta(double rho = 0.95, double epscut = 1.0e-7)
      : rho_(rho), epscut_(epscut), I_(0, 1) {
    npar_ = -1;

    PrintParameters();
  }

  // Json constructor
  explicit AdaDelta(const json &pars) : I_(0, 1) {
    npar_ = -1;

    from_json(pars);
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Adadelta optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Rho = " << rho_ << std::endl;
    InfoMessage() << "Epscut = " << epscut_ << std::endl;
  }

  void Init(const Eigen::VectorXd &pars) override {
    npar_ = pars.size();
    Eg2_.setZero(npar_);
    Edx2_.setZero(npar_);
  }

  void Init(const Eigen::VectorXcd &pars) override {
    npar_ = 2 * pars.size();
    Eg2_.setZero(npar_);
    Edx2_.setZero(npar_);
  }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) override {
    assert(npar_ > 0);

    Eg2_ = rho_ * Eg2_ + (1. - rho_) * grad.cwiseAbs2();

    Eigen::VectorXd Dx(npar_);

    for (int i = 0; i < npar_; i++) {
      Dx(i) = -std::sqrt(Edx2_(i) + epscut_) * grad(i);
      Dx(i) /= std::sqrt(Eg2_(i) + epscut_);
      pars(i) += Dx(i);
    }

    Edx2_ = rho_ * Edx2_ + (1. - rho_) * Dx.cwiseAbs2();
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXd &pars) override {
    Update(Eigen::VectorXd(grad.real()), pars);
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXcd &pars) override {
    assert(npar_ == 2 * pars.size());

    Eigen::VectorXd Dx(npar_);

    for (int i = 0; i < pars.size(); i++) {
      Eg2_(2 * i) =
          rho_ * Eg2_(2 * i) + (1. - rho_) * std::pow(grad(i).real(), 2);
      Eg2_(2 * i + 1) =
          rho_ * Eg2_(2 * i + 1) + (1. - rho_) * std::pow(grad(i).imag(), 2);

      Dx(2 * i) = -std::sqrt(Edx2_(2 * i) + epscut_) * grad(i).real();
      Dx(2 * i + 1) = -std::sqrt(Edx2_(2 * i + 1) + epscut_) * grad(i).imag();
      Dx(2 * i) /= std::sqrt(Eg2_(2 * i) + epscut_);
      Dx(2 * i + 1) /= std::sqrt(Eg2_(2 * i + 1) + epscut_);

      pars(i) += Dx(2 * i);
      pars(i) += I_ * Dx(2 * i + 1);

      Edx2_(2 * i) = rho_ * Edx2_(2 * i) + (1. - rho_) * std::pow(Dx(2 * i), 2);
      Edx2_(2 * i + 1) =
          rho_ * Edx2_(2 * i + 1) + (1. - rho_) * std::pow(Dx(2 * i + 1), 2);
    }
  }

  void Reset() override {
    Eg2_ = Eigen::VectorXd::Zero(npar_);
    Edx2_ = Eigen::VectorXd::Zero(npar_);
  }

  void from_json(const json &pars) {
    // DEPRECATED (to remove for v2.0.0)
    std::string section = "Optimizer";
    if (!FieldExists(pars, section)) {
      section = "Learning";
    }
    rho_ = FieldOrDefaultVal(pars[section], "Rho", 0.95);
    epscut_ = FieldOrDefaultVal(pars[section], "Epscut", 1.0e-7);
  }
};

}  // namespace netket

#endif


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

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/index/rtree.hpp>

#include "../../RTree/RTree.h"
#ifdef RTREE_H
#undef RTREE_H
#endif

#include "../../DynDimRTree/RTree.h"

#ifndef DIM
#define DIM 4
#endif
#ifndef STRATEGY
#define STRATEGY quadratic<16>
#endif
#define NUM_BOUNDARY_POINTS 4096
#define NUM_TEST_OBJECTS 50000

typedef boost::geometry::model::point<double, DIM, boost::geometry::cs::cartesian> bg_point;
typedef boost::geometry::model::box<bg_point> bg_box;
typedef std::pair<bg_box, size_t> bg_value;

double drand() {
    return ((double)rand() / (double)(RAND_MAX));
}

bool leq (const double * const p1, const double * const p2) {
    for(size_t i=0; i<DIM; i++) {
        if (p1[i] > p2[i]) return false;
    }
    return true;
}

bool incomparable (const double border[][DIM], const double p[DIM], const size_t border_size) {
    for(size_t i=0; i<border_size; i++) {
        if (leq(border[i],p))
            return false;
        if (leq(p,border[i]))
            return false;
    }
    return true;
}

bool point_above (const double border[][DIM], const double p[DIM], const size_t border_size) {
    for(size_t i=0; i<border_size; i++) {
        if (leq(border[i],p))
            return true;
    }
    return false;
}

inline bool MySearchCallback(size_t)
{
    return false;
}

class GenerateBorder {
public:
    GenerateBorder(double p0[DIM], double border[][DIM], size_t border_size) {
        for(size_t i=0; i<DIM; i++) p0[i]=0;
        std::cout << "Generating border with number of points=" << border_size << std::endl;
        for(size_t i=0; i<border_size; i++) {
            do {
                double sum=0;
                for(size_t j=0; j<DIM; j++) { border[i][j]=drand(); sum+=border[i][j]; }
                const double squeeze_factor = 1 + 1.407777778*drand();
                for(size_t j=0; j<DIM; j++) border[i][j]=std::min<double>(squeeze_factor*border[i][j]/sum,1);
            } while (!incomparable(border,border[i],i));
        }
        std::cout << "Border created" << std::endl;
    }
};

class GenerateRtree {
public:
    GenerateRtree(RTree<size_t,double,DIM> & gh_rtree,
                  DynDimRTree::RTree<size_t,double> & dyndim_rtree,
                  boost::geometry::index::rtree< bg_value, boost::geometry::index::STRATEGY > & bg_rtree,
                  const double border[][DIM], size_t border_size) {
        for(size_t i=0; i<border_size; i++) {
            gh_rtree.Insert(border[i],border[i],i);
            dyndim_rtree.Insert(border[i],border[i],i);

            bg_point bgp;
            auto bgp_data = &bgp.get<0>();
            memcpy((double *)bgp_data, border[i], DIM*sizeof(double)); //ugly cast
            const bg_box b(bgp,bgp);
            bg_rtree.insert(std::make_pair(b,i));
        }
    }
};

class GenerateData2Test {
public:
    GenerateData2Test(double point2test[][DIM],
                      boost::geometry::index::detail::predicates::spatial_predicate
                              <bg_box, boost::geometry::index::detail::predicates::intersects_tag, false> bg_predicate[],
                              const size_t points2test_size) {
        bg_point bg_point0;
        auto bg_point0_data = &bg_point0.get<0>();
        memset((double *)bg_point0_data, 0, DIM*sizeof(double)); //ugly cast
        for(size_t i=0; i<points2test_size; i++) {
            for(size_t j=0; j<DIM; j++) {
                point2test[i][j]=drand();
            }
            bg_point bgp;
            auto bgp_data = &bgp.get<0>();
            memcpy((double *)bgp_data, point2test[i], DIM*sizeof(double)); //ugly cast
            bg_predicate[i] = boost::geometry::index::intersects(bg_box(bg_point0,bgp));
        }
    }
};

double p0[DIM];
static double border[NUM_BOUNDARY_POINTS][DIM];
static GenerateBorder generateBorder(p0, border, NUM_BOUNDARY_POINTS);

// github rtree: https://github.com/nushoin/RTree.git
// known drawbacks:
// - Unit sphere volumes are not computed for DIM > 20
// - dim is a template parameter
RTree<size_t, double, DIM> gh_rtree_1k, gh_rtree_2k, gh_rtree_4k;
DynDimRTree::RTree<size_t, double> dyndim_rtree_1k(DIM), dyndim_rtree_2k(DIM), dyndim_rtree_4k(DIM);

// boost rtree
// known drawbacks: slow for my use case?
boost::geometry::index::rtree< bg_value, boost::geometry::index::STRATEGY > bg_rtree_1k, bg_rtree_2k, bg_rtree_4k;
static auto bg_rtree_1k_qend = bg_rtree_1k.qend();
static auto bg_rtree_2k_qend = bg_rtree_2k.qend();
static auto bg_rtree_4k_qend = bg_rtree_4k.qend();

GenerateRtree generateRtree_1k(gh_rtree_1k, dyndim_rtree_1k, bg_rtree_1k, border, NUM_BOUNDARY_POINTS/4);
GenerateRtree generateRtree_2k(gh_rtree_2k, dyndim_rtree_2k, bg_rtree_2k, border, NUM_BOUNDARY_POINTS/2);
GenerateRtree generateRtree_4k(gh_rtree_4k, dyndim_rtree_4k, bg_rtree_4k, border, NUM_BOUNDARY_POINTS);

static double point2test[NUM_TEST_OBJECTS][DIM];
static boost::geometry::index::detail::predicates::spatial_predicate
        <bg_box, boost::geometry::index::detail::predicates::intersects_tag, false> bg_predicate[NUM_TEST_OBJECTS];
GenerateData2Test generateData2Test(point2test, bg_predicate, NUM_TEST_OBJECTS);

BOOST_AUTO_TEST_CASE( test_rtree4d_consistency ) {
    BOOST_CHECK(point_above (border, border[10], NUM_BOUNDARY_POINTS/2) );
    BOOST_CHECK_EQUAL(gh_rtree_2k.Search(p0, border[10], MySearchCallback), 1 );

    size_t above_count = 0, below_count=0;
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(size_t i=0; i<NUM_TEST_OBJECTS; i++) {
        const bool above_slow = point_above (border, point2test[i], NUM_BOUNDARY_POINTS/2);
        const bool above_gh = gh_rtree_2k.Search(p0, point2test[i], MySearchCallback) > 0;
        const bool above_tmcombi = dyndim_rtree_2k.Search(p0, point2test[i], MySearchCallback) > 0;
        const bool above_bg = bg_rtree_2k.qbegin(bg_predicate[i]) != bg_rtree_2k_qend;

        if (above_gh && !above_slow) {
            BOOST_FAIL("gh_rtree reports above, but not the point_above");
            exit(-1);
        }
        if (!above_gh && above_slow) {
            BOOST_FAIL("point_above reports true, but not the gh_rtree");
            exit(-1);
        }
        if (above_tmcombi && !above_slow) {
            BOOST_FAIL("dyndim_rtree reports above, but not the point_above");
            exit(-1);
        }
        if (!above_tmcombi && above_slow) {
            BOOST_FAIL("point_above reports true, but not the dyndim_rtree");
            exit(-1);
        }
        if (above_bg && !above_slow) {
            BOOST_FAIL("bg_rtree reports above, but not the point_above");
            exit(-1);
        }
        if (!above_bg && above_slow) {
            BOOST_FAIL("point_above reports true, but not the bg_rtree");
            exit(-1);
        }
        if (above_slow)
            above_count++;
        else
            below_count++;
    }
    BOOST_TEST_MESSAGE("below_count="	<< below_count << ", above_count=" << above_count);
}

BOOST_AUTO_TEST_CASE( test4d_streight_check_1k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_slow = point_above (border, i, NUM_BOUNDARY_POINTS/4);
        (void)above_slow;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_streight_check_2k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_slow = point_above (border, i, NUM_BOUNDARY_POINTS/2);
        (void)above_slow;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_streight_check_4k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_slow = point_above (border, i, NUM_BOUNDARY_POINTS);
        (void)above_slow;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_gh_rtree_check_1k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_gh = gh_rtree_1k.Search(p0, i, MySearchCallback) > 0;
        (void)above_gh;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_gh_rtree_check_2k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_gh = gh_rtree_2k.Search(p0, i, MySearchCallback) > 0;
        (void)above_gh;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_gh_rtree_check_4k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_gh = gh_rtree_4k.Search(p0, i, MySearchCallback) > 0;
        (void)above_gh;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_dyndim_rtree_check_1k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_tmcombi = dyndim_rtree_1k.Search(p0, i, MySearchCallback) > 0;
        (void)above_tmcombi;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_dyndim_rtree_check_2k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_tmcombi = dyndim_rtree_2k.Search(p0, i, MySearchCallback) > 0;
        (void)above_tmcombi;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_dyndim_rtree_check_4k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : point2test) {
        const bool above_tmcombi = dyndim_rtree_4k.Search(p0, i, MySearchCallback) > 0;
        (void)above_tmcombi;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_bg_rtree_check_1k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : bg_predicate) {
        const bool above_bg = bg_rtree_1k.qbegin(i) != bg_rtree_1k_qend;
        (void)above_bg;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_bg_rtree_check_2k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : bg_predicate) {
        const bool above_bg = bg_rtree_2k.qbegin(i) != bg_rtree_2k_qend;
        (void)above_bg;
    }
    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_CASE( test4d_bg_rtree_check_4k ) {
    BOOST_TEST_MESSAGE("Input objects to test="	<< NUM_TEST_OBJECTS);
    for(auto & i : bg_predicate) {
        const bool above_bg = bg_rtree_4k.qbegin(i) != bg_rtree_4k_qend;
        (void)above_bg;
    }
    BOOST_CHECK(true);
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file flatextrapolation.hpp
    \brief flat interpolation decorator
    \ingroup math
*/

#ifndef quantext_flat_extrapolation_hpp
#define quantext_flat_extrapolation_hpp

#include <ql/math/interpolation.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Flat extrapolation given a base interpolation
/*! \ingroup math
 */
class FlatExtrapolation : public Interpolation {
private:
    class FlatExtrapolationImpl : public Interpolation::Impl {

    public:
        FlatExtrapolationImpl(const boost::shared_ptr<Interpolation>& i) : i_(i) {}
        void update() { i_->update(); }
        Real xMin() const { return i_->xMin(); }
        Real xMax() const { return i_->xMax(); }
        std::vector<Real> xValues() const { QL_FAIL("not implemented"); }
        std::vector<Real> yValues() const { QL_FAIL("not implemented"); }
        bool isInRange(Real x) const { return i_->isInRange(x); }
        Real value(Real x) const {
            Real tmp = std::max(std::min(x, i_->xMax()), i_->xMin());
            return i_->operator()(tmp);
        }
        Real primitive(Real x) const {
            if (x >= i_->xMin() && x <= i_->xMax()) {
                return i_->primitive(x);
            }
            if (x < i_->xMin()) {
                return i_->primitive(i_->xMin()) - (i_->xMin() - x);
            } else {
                return i_->primitive(i_->xMax()) + (x - i_->xMax());
            }
        }
        Real derivative(Real x) const {
            if (x > i_->xMin() && x < i_->xMax()) {
                return i_->derivative(x);
            } else {
                // that is the left derivative for xmin and
                // the right derivative for xmax
                return 0.0;
            }
        }
        Real secondDerivative(Real x) const {
            if (x > i_->xMin() && x < i_->xMax()) {
                return i_->secondDerivative(x);
            } else {
                // that is the left derivative for xmin and
                // the right derivative for xmax
                return 0.0;
            }
        }

    private:
        const boost::shared_ptr<Interpolation> i_;
    };

public:
    FlatExtrapolation(const boost::shared_ptr<Interpolation>& i) {
        impl_ = boost::make_shared<FlatExtrapolationImpl>(i);
        impl_->update();
    }
};

//! %Linear-interpolation and flat extrapolation factory and traits
class LinearFlat {
public:
    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return FlatExtrapolation(boost::make_shared<LinearInterpolation>(xBegin, xEnd, yBegin));
    }
    static const bool global = false;
    static const Size requiredPoints = 2;
};

//! %Linear-interpolation and flat extrapolation factory and traits
class LogLinearFlat {
public:
    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return FlatExtrapolation(boost::make_shared<LogLinearInterpolation>(xBegin, xEnd, yBegin));
    }
    static const bool global = false;
    static const Size requiredPoints = 2;
};

//! Hermite interpolation and flat extrapolation factory and traits
class HermiteFlat {
public:
     template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
         return FlatExtrapolation(boost::make_shared<Parabolic>(xBegin, xEnd, yBegin));
     }
     static const bool global = false;
     static const Size requiredPoints = 2;
};

//! Cubic interpolation and flat extrapolation factory and traits
class CubicFlat {
public:
    CubicFlat(
        QuantLib::CubicInterpolation::DerivativeApprox da = QuantLib::CubicInterpolation::Kruger,
        bool monotonic = false,
        QuantLib::CubicInterpolation::BoundaryCondition leftCondition = QuantLib::CubicInterpolation::SecondDerivative,
        QuantLib::Real leftConditionValue = 0.0,
        QuantLib::CubicInterpolation::BoundaryCondition rightCondition = QuantLib::CubicInterpolation::SecondDerivative,
        QuantLib::Real rightConditionValue = 0.0)
        : da_(da), monotonic_(monotonic), leftType_(leftCondition), rightType_(rightCondition),
          leftValue_(leftConditionValue), rightValue_(rightConditionValue) {}

    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return FlatExtrapolation(boost::make_shared<CubicInterpolation>(
            xBegin, xEnd, yBegin, da_, monotonic_, leftType_, leftValue_, rightType_, rightValue_));
    }

    static const bool global = true;
    static const Size requiredPoints = 2;

private:
    QuantLib::CubicInterpolation::DerivativeApprox da_;
    bool monotonic_;
    QuantLib::CubicInterpolation::BoundaryCondition leftType_;
    QuantLib::CubicInterpolation::BoundaryCondition rightType_;
    QuantLib::Real leftValue_;
    QuantLib::Real rightValue_;
};

} // namespace QuantExt

#endif


#ifndef POLYLINECOMPRESSOR_H_
#define POLYLINECOMPRESSOR_H_

#include "util/coordinate.hpp"

#include <algorithm>
#include <boost/assert.hpp>
#include <string>
#include <vector>

namespace osrm
{
namespace engine
{
namespace detail
{
constexpr double POLYLINE_DECODING_PRECISION = 1e5;
constexpr double POLYLINE_TO_COORDINATE = COORDINATE_PRECISION / POLYLINE_DECODING_PRECISION;

std::string encode(std::vector<int> &numbers);
}
using CoordVectorForwardIter = std::vector<util::Coordinate>::const_iterator;
// Encodes geometry into polyline format.
// See: https://developers.google.com/maps/documentation/utilities/polylinealgorithm

template <unsigned POLYLINE_PRECISION = 100000>
std::string encodePolyline(CoordVectorForwardIter begin, CoordVectorForwardIter end)
{
    double coordinate_to_polyline = POLYLINE_PRECISION / COORDINATE_PRECISION;
    auto size = std::distance(begin, end);
    if (size == 0)
    {
        return {};
    }

    std::vector<int> delta_numbers;
    BOOST_ASSERT(size > 0);
    delta_numbers.reserve((size - 1) * 2);
    int current_lat = 0;
    int current_lon = 0;
    std::for_each(
        begin,
        end,
        [&delta_numbers, &current_lat, &current_lon, coordinate_to_polyline](
            const util::Coordinate loc) {
            const int lat_diff =
                std::round(static_cast<int>(loc.lat) * coordinate_to_polyline) - current_lat;
            const int lon_diff =
                std::round(static_cast<int>(loc.lon) * coordinate_to_polyline) - current_lon;
            delta_numbers.emplace_back(lat_diff);
            delta_numbers.emplace_back(lon_diff);
            current_lat += lat_diff;
            current_lon += lon_diff;
        });
    return detail::encode(delta_numbers);
}

// Decodes geometry from polyline format
// See: https://developers.google.com/maps/documentation/utilities/polylinealgorithm
std::vector<util::Coordinate> decodePolyline(const std::string &polyline);
}
}

#endif /* POLYLINECOMPRESSOR_H_ */


#include <stdio.h>
#include <inttypes.h>
#include <fstream>
#include <iostream>
#include <vector>
#include <signal.h>
#include <getopt.h>

#include <lcm/lcm.h>
#include <bot_core/bot_core.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <GL/gl.h>

#include <Eigen/Dense>

#include <lcmtypes/bot_core_pose_t.h>
#include <lcmtypes/bot_core_image_t.h>

namespace rv{

void draw_line_lcmgl(bot_lcmgl_t* lcmgl_, Eigen::Vector3d st_pt, Eigen::Vector3d end_pt,
        float r, float g, float b){

    // position

    bot_lcmgl_line_width(lcmgl_, 4.0f);
    bot_lcmgl_point_size(lcmgl_, 7.0f);
    bot_lcmgl_color3f(lcmgl_, r, g, b);

    bot_lcmgl_begin(lcmgl_, GL_LINE_STRIP);
    bot_lcmgl_vertex3d(lcmgl_, st_pt[0], st_pt[1], st_pt[2]);
    bot_lcmgl_vertex3d(lcmgl_,end_pt[0], end_pt[1], end_pt[2]);
    bot_lcmgl_end(lcmgl_);

}

void draw_pose_lcmgl(bot_lcmgl_t* lcmgl_, Eigen::Matrix4d pose,
			float r, float g, float b){

	float normal_length = 0.4;

	bot_lcmgl_translated(lcmgl_, 0, 0, 0);
	bot_lcmgl_line_width(lcmgl_, 2.0f);
	bot_lcmgl_point_size(lcmgl_, 7.0f);
	bot_lcmgl_color3f(lcmgl_, r, g, b);

	Eigen::Vector3d pt;
	pt << pose(0, 3), pose(1, 3), pose(2, 3);

	Eigen::Matrix4d M_fd, M_left, M_up;
	Eigen::Matrix4d T_fd, T_left, T_up;
	T_fd << 1, 0, 0, normal_length,
			0, 1, 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
	M_fd = pose * T_fd;

	T_left << 	1, 0, 0, 0,
			0, 1, 0, normal_length/2,
			0, 0, 1, 0,
			0, 0, 0, 1;
	M_left = pose * T_left;

	T_up << 1, 0, 0, 0,
			0, 1, 0, 0,
			0, 0, 1, normal_length/2,
			0, 0, 0, 1;
	M_up = pose * T_up;

	Eigen::Vector3d pt_fd, pt_left, pt_up;
	pt_fd << M_fd(0, 3), M_fd(1, 3), M_fd(2, 3);
	pt_left << M_left(0, 3), M_left(1, 3), M_left(2, 3);
	pt_up << M_up(0, 3), M_up(1, 3), M_up(2, 3);

	bot_lcmgl_begin(lcmgl_, GL_LINE_STRIP);
	bot_lcmgl_vertex3f(lcmgl_, pt[0], pt[1], pt[2]);
	bot_lcmgl_vertex3f(lcmgl_, pt_fd[0], pt_fd[1], pt_fd[2]);
	bot_lcmgl_end(lcmgl_);

	bot_lcmgl_begin(lcmgl_, GL_LINE_STRIP);
	bot_lcmgl_vertex3f(lcmgl_, pt[0], pt[1], pt[2]);
	bot_lcmgl_vertex3f(lcmgl_, pt_left[0], pt_left[1], pt_left[2]);
	bot_lcmgl_end(lcmgl_);

	bot_lcmgl_begin(lcmgl_, GL_LINE_STRIP);
	bot_lcmgl_vertex3f(lcmgl_, pt[0], pt[1], pt[2]);
	bot_lcmgl_vertex3f(lcmgl_, pt_up[0], pt_up[1], pt_up[2]);
	bot_lcmgl_end(lcmgl_);

	bot_lcmgl_begin(lcmgl_, GL_POINTS);
	bot_lcmgl_vertex3f(lcmgl_, pt[0], pt[1], pt[2]);
	bot_lcmgl_end(lcmgl_);
}

} // end of namespace rv

static void
usage(const char *name)
{
    fprintf(
            stderr,
            "usage: %s [options]\n"
            "\n"
            "  -h, --help                            Shows this help text and exits\n"
            "\n",
            name);

    exit(1);
}

int main(int argc, char** argv)
{

    const char *optstring = "h";
    int c;
    struct option long_opts[] =
    {
        { "help", no_argument, 0, 'h' },
        { 0, 0, 0, 0 }
    };

    while ((c = getopt_long(argc, argv, optstring, long_opts, 0)) >= 0) {
        switch (c){
            case 'h':
                usage(argv[0]);
                break;
        }
    }

    lcm_t* lcm_ = lcm_create(NULL);
    bot_lcmgl_t* lcmgl_pose = bot_lcmgl_init(lcm_, "LCMGL_POSE_DEMO");

    // Draw a pose at origin
    Eigen::Matrix4d P_o;
	P_o <<  1, 0, 0, 0,
			0, 1, 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
	rv::draw_pose_lcmgl(lcmgl_pose, P_o, 1, 0, 0);

	// Do a forward (at X axis)
	// and 90 degree yaw transformation
	Eigen::Matrix4d T_x;
	T_x <<  0,-1, 0, 1,
			1, 0, 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1;
	Eigen::Matrix4d P_1;
	P_1 = P_o * T_x;
	rv::draw_pose_lcmgl(lcmgl_pose, P_1, 0, 1, 0);

    bot_lcmgl_switch_buffer(lcmgl_pose);

    return 0;
}


#ifndef __ECTS_CONTROL_ALGORITHM__
#define __ECTS_CONTROL_ALGORITHM__

#include <ros/ros.h>
#include <Eigen/Dense>
#include <math.h>
#include <pr2_algorithms/algorithm_base.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <boost/thread.hpp>
#include <limits>
#include <stdexcept>
#include <cmath>
#include <dynamic_reconfigure/server.h>
#include <pr2_algorithms/ectsConfig.h>

namespace manipulation_algorithms{
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 6, 7> Matrix67d;
typedef Eigen::Matrix<double, 12, 12> Matrix12d;
typedef Eigen::Matrix<double, 14, 14> Matrix14d;
typedef Eigen::Matrix<double, 6, 14> MatrixECTSr;
typedef Eigen::Matrix<double, 12, 14> MatrixECTS;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::Matrix<double, 7, 1> Vector7d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 12, 1> Vector12d;
typedef Eigen::Matrix<double, 14, 1> Vector14d;

  /**
    Class that implements an extended cooperative task space (ECTS) control algorithm.

    Hardcoded dimensions to fit two 7 DOF manipulators.
  **/
  class ECTSController : public AlgorithmBase
  {
  public:
    ECTSController(const KDL::Chain &chain_1, const KDL::Chain &chain_2);
    ~ECTSController();

    virtual bool getParams(const ros::NodeHandle &n);

    /**
      Computes the ECTS reference joint velocities for the two manipulators.

      @param r_i The virtual sticks connecting the manipulators end-effectos to the object reference points.
      @param q_dot_i The manipulators' joint velocities.
      @param twist_a The commanded absolute motion twist.
      @param twist_r The commanded relative motion twist.
      @return The 14 dimensional joint velocities vector.
    **/
    Vector14d control(const Vector3d &r_1, const Vector3d &r_2, const KDL::JntArray &q1, const KDL::JntArray &q2, const Vector6d &twist_a, const Vector6d &twist_r);

    /**
      Adds a task relevant direction, u. The controller will try to optimize
      its velocity transmission ratio. Adding multiple u's will make the controller
      try to optimize the motion task compatibility index defined as

      \f$c_m = \sum_{i=1}^{n} \alpha_{m_i}^{\pm 2}\f$

      where

      \f$\alpha_{m_i} = [u_{m_i}^\top (J_{E}J_{E}^\top)^{-1}u_{m_i}]^{-1/2}\f$

      @param u The \f$u_{m_i}\f$ to be added.
    **/
    void addOptimizationDirection(const Vector12d &u);

    /**
      Resets the vector of optimization directions.
    **/
    void clearOptimizationDirections();

    /**
      Return the last computed task compatibility value.
    **/
    double getTaskCompatibility();

    /**
      Return the current alpha value that determines the degree of colaboration between arms.
    **/
    double getAlpha();

    /**
      Sets the alpha value.

      @param alpha The desired ECTS alpha value.
    **/
    void setAlpha(double alpha);

    /**
      Sets the gradient ascent gain for the nullspace optimization task.

      @param km The desired nullspace optimization gain.
    **/
    void setNullspaceGain(double km);
  private:
    double alpha_, damping_, current_cm_, km_, optimization_hz_, gradient_delta_, max_nullspace_velocities_, joint_updt_threshold_;
    Matrix12d K_;
    int beta_, joint_threshold_count_lim_;
    boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_1_;
    boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver_2_;
    std::vector<Vector12d> u_list_;
    KDL::JntArray q1_, q2_;
    Vector3d r_1_, r_2_;
    Vector14d epsilon_, prev_out_;
    std::vector<int> thres_count_;
    boost::thread optimization_thread_;
    boost::mutex optimization_mutex_;
    boost::shared_ptr<dynamic_reconfigure::Server<pr2_algorithms::ectsConfig> > dyncfg_server_;
    dynamic_reconfigure::Server<pr2_algorithms::ectsConfig>::CallbackType dyncfg_cb_;

    /**
      Compute the velocity transmission ratio along the direction u.

      @param J The ECTS jacobian of the system.
      @param u The direction along which to measure the velocity transmission ratio.
    **/
    double computeTransmissionRatio(const MatrixECTS &J, const Vector12d &u);

    /**
      Computes the ECTS jacobian that maps joints to task space twists.

      @return The ECTS jacobian.
    **/
    MatrixECTS computeECTSJacobian(const KDL::JntArray &q1, const KDL::JntArray &q2);

    /**
      Computes the task compatibility measure \f$c_m = \sum_{i=1}^n \alpha_{m_i}^{\pm2}\f$.
    **/
    double computeTaskCompatibility(const MatrixECTS &J);

    /**
      Computes the nullspace optimization task by numerically estimating the gradient of the task compatibility measure.

      @return The 14-dimensional joint velocity vector representing the desired motion, which must be projected onto the nullspace of the
      task jacobian.
    **/
    Vector14d computeNullSpaceTask();

    /**
      Loop that uptades the nullspace task value, meant to be run assynchronously to avoid breaking
      realtime constraints.
    **/
    void optimizationTaskLoop();

    /**
      Dynamic reconfigure callback allows for online tuning of controller gains.
    **/
    void reconfigureCallback(const pr2_algorithms::ectsConfig &config, uint32_t level);
  };
}
#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
//
// 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.
//---------------------------------------------------------------------------//

#define PROVING_KEY_FILE "provkey.bin"
#define VERIFICATION_KEY_FILE "verifkey.bin"
#define BIG_PROOF_FILE "big_proof.bin"
#define PROOF_FILE "proof.bin"
#define PRIMARY_INPUT_FILE "primary_input.bin"

#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "detail/r1cs_examples.hpp"
#include "detail/sha256_component.hpp"

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/fields/bls12/base_field.hpp>
#include <nil/crypto3/algebra/fields/bls12/scalar_field.hpp>
#include <nil/crypto3/algebra/fields/arithmetic_params/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/multiexp/bls12.hpp>
#include <nil/crypto3/algebra/curves/params/wnaf/bls12.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>

#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark.hpp>
#include <nil/crypto3/zk/snark/schemes/ppzksnark/r1cs_gg_ppzksnark/marshalling.hpp>

#include <nil/crypto3/zk/snark/algorithms/generate.hpp>
#include <nil/crypto3/zk/snark/algorithms/verify.hpp>
#include <nil/crypto3/zk/snark/algorithms/prove.hpp>

#include <nil/crypto3/zk/components/component.hpp>

#include <nil/crypto3/zk/components/blueprint.hpp>
#include <nil/crypto3/zk/components/blueprint_variable.hpp>
// #include <nil/crypto3/zk/snark/components/basic_components.hpp>

// #include <nil/crypto3/zk/snark/relations/constraint_satisfaction_problems/r1cs.hpp>

using namespace nil::crypto3;
using namespace nil::crypto3::zk;
using namespace nil::crypto3::zk::components;
using namespace nil::crypto3::algebra;

#include <iostream>
#include <string>
#include "picosha2.hpp"

using namespace std;

typedef algebra::curves::bls12<381> curve_type;
typedef typename curve_type::scalar_field_type field_type;
typedef zk::snark::r1cs_gg_ppzksnark<curve_type> scheme_type;

using std::string;
using std::cout;
using std::endl;

using curve_type = curves::bls12<381>;
using field_type = typename curve_type::scalar_field_type;

// must be done here, after definition of curve_type and field_type
#include "utils.hpp"

void build_circuit ( blueprint<field_type> &bp, std::vector<uint32_t> &passphrase32 ){

  blueprint_variable<field_type> public_pubkey0;
  public_pubkey0.allocate( bp );

  blueprint_variable<field_type> public_pubkey1;
  public_pubkey1.allocate( bp );

  blueprint_variable<field_type> public_pubkey2;
  public_pubkey2.allocate( bp );

  blueprint_variable<field_type> public_pubkey3;
  public_pubkey3.allocate( bp );

  blueprint_variable<field_type> secret_pubkey0;
  secret_pubkey0.allocate( bp );

  blueprint_variable<field_type> secret_pubkey1;
  secret_pubkey1.allocate( bp );

  blueprint_variable<field_type> secret_pubkey2;
  secret_pubkey2.allocate( bp );

  blueprint_variable<field_type> secret_pubkey3;
  secret_pubkey3.allocate( bp );

  blueprint_variable<field_type> secret_hash0;
  secret_hash0.allocate( bp );

  blueprint_variable<field_type> secret_hash1;
  secret_hash1.allocate( bp );

  blueprint_variable<field_type> secret_hash2;
  secret_hash2.allocate( bp );

  blueprint_variable<field_type> secret_hash3;
  secret_hash3.allocate( bp );

  bp.set_input_sizes(4);

  // This sets up the blueprint variables
  // so that the first one (out) represents the public
  // input and the rest is private input

  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey0 , 1, public_pubkey0));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey1 , 1, public_pubkey1));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey2 , 1, public_pubkey2));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( secret_pubkey3 , 1, public_pubkey3));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[0] , 1, secret_hash0));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[1] , 1, secret_hash1));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[2] , 1, secret_hash2));
  bp.add_r1cs_constraint(r1cs_constraint<field_type>( passphrase32[3] , 1, secret_hash3));
}

void prepare_circuit ( std::string passphrase ){

  using curve_type = curves::bls12<381>;
  using field_type = typename curve_type::scalar_field_type;

  std::vector<uint32_t> passphrase32(4);
  uint32s_of_passphrase( passphrase32, passphrase );

  blueprint<field_type> bp;
  build_circuit( bp, passphrase32 );

  const r1cs_constraint_system<field_type> constraint_system =
    bp.get_constraint_system();
  const typename r1cs_gg_ppzksnark<curve_type>::keypair_type keypair =
    generate<r1cs_gg_ppzksnark<curve_type>>(constraint_system);

  save_proving_key( PROVING_KEY_FILE, keypair.first ) ;
  save_verification_key( VERIFICATION_KEY_FILE , keypair.second );

}

void use_circuit( std::string passphrase, std::string pubkey ){

  std::vector<uint32_t> passphrase32(4);
  std::vector<uint32_t> pubkey32(4);

  if( pubkey.size() != 64 ){
    cerr << "Wrong size for pubkey, should be 64 hexa chars" << endl ;
    exit(2) ;
  }

  uint32s_of_passphrase( passphrase32, passphrase );
  uint32s_of_pubkey( pubkey32, pubkey );

  // public
  snark::r1cs_variable_assignment<field_type> primary_assignment;
  for( int i = 0; i < 4; i++){
    primary_assignment.push_back( pubkey32 [i] );
  }
  typename scheme_type::primary_input_type primary_input
    ( primary_assignment.begin(), primary_assignment.begin() + 4 );

  // private
  snark::r1cs_variable_assignment<field_type> auxiliary_assignment;
  for( int i = 0; i < 4; i++){
    auxiliary_assignment.push_back( pubkey32 [i] );
  }
  for( int i = 0; i < 4; i++){
    auxiliary_assignment.push_back( passphrase32 [i] );
  }

  typename scheme_type::auxiliary_input_type auxiliary_input
    ( auxiliary_assignment.begin(), auxiliary_assignment.begin() + 8 );

  typename scheme_type::proving_key_type pk =
    load_proving_key( PROVING_KEY_FILE );

  const typename r1cs_gg_ppzksnark<curve_type>::proof_type proof =
    prove<r1cs_gg_ppzksnark<curve_type>>( pk,  primary_input, auxiliary_input );

  const typename r1cs_gg_ppzksnark<curve_type>::verification_key_type vk =
    load_verification_key( VERIFICATION_KEY_FILE );

  bool verified =
    verify<r1cs_gg_ppzksnark<curve_type>>(vk, primary_input, proof);

  if( !verified ){
    cerr << "Invalid proof" << endl;
    exit(2) ;
  }

  save_primary_input( PRIMARY_INPUT_FILE, primary_input );
  save_big_proof( BIG_PROOF_FILE, proof, primary_input, vk );
  save_proof( PROOF_FILE, proof );

}

void print_usage(std::ostream &cout, std::string command )
{
    cout << command << " SUBCOMMAND [ARGUMENTS]" << endl ;
    cout << "Available subcommands:" << endl ;
    cout << "  * prepare PASSPHRASE : output circuit for PASSPHRASE to 'provkey.bin' and 'verifkey.bin'" << endl ;
    cout << "  * prove PASSPHRASE PUBKEY : generate 'proof.bin', 'verifkey.bin', 'variables.bin' and 'big_proof.bin'" << endl ;
    cout << "Arguments:" << endl ;
    cout << "  PASSPHRASE: an ASCII string that you have to memorize" << endl ;
    cout << "  PUBKEY: the Hexadecimal representation of your pubkey (without 0x)" << endl ;

}

int main(int argc, char *argv[]) {

  if( argc == 1 ){
    print_usage ( cout, argv[0] ) ;
    return 0 ;
  }

  std::string subcommand = argv[1] ;
  if( subcommand == "prepare" ){
    cerr << "prepare" << endl ;
    if( argc != 3 ){
      cerr << "Bad number of arguments:" << endl ;
      print_usage ( cerr, argv[0] ) ;
      return 1;
    }

    prepare_circuit( argv[2] );
    return 0 ;
  } else
  if( subcommand == "prove" ){
    cerr << "prove" << endl ;
    if( argc != 4 ){
      cerr << "Bad number of arguments:" << endl ;
      print_usage ( cerr, argv[0] ) ;
      return 1;
    }
    use_circuit( argv[2], argv[3] );
    return 0 ;
  } else {
    cerr << "Unknown subcommand: " << subcommand << endl;
    return 2 ;
  }

}

/*
*/


#ifndef GRAPHS_UTILS_HPP
#define GRAPHS_UTILS_HPP

#include <utility>
#include <vector>
#include <fstream>
#include <filesystem>
#include <numeric>

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>

#include <osmium/osm/node_ref.hpp>
#include <osmium/osm/way.hpp>

namespace graphs {
using Distance = double;
using Angle = long double;
using Location = std::pair<Angle, Angle>;
using Locations = std::vector<Location>;

template<typename T>
bool serialize(const std::string& filename, T&& data) {
    std::ofstream binary { filename, std::ios::out | std::ios::binary | std::ios::app };
    boost::archive::binary_oarchive archive { binary, boost::archive::no_header };
    archive << data;
    binary.close();
    return true;
}

template<typename T>
bool deserialize(const std::string& filename, T&& data) {
    if (!std::filesystem::exists(filename)) { return false; }
    std::ifstream binary { filename, std::ios::binary };
    boost::archive::binary_iarchive archive { binary, boost::archive::no_header };
    archive >> data;
    binary.close();
    return true;
}

/**
 * Factory method for Position.
 */
inline auto make_pos(const osmium::NodeRef& node) -> Location {
    return { node.lat(), node.lon() };
}

/**
 * Determines the great-circle distance between two points given their longitudes and latitudes.
 *
 * @param x, y OSM nodes with corresponding coordinates.
 * @return Distance between nodes.
 */
inline auto haversine(const Location& x, const Location& y) -> Distance {
    const auto[lat_1, lon_1] = x;
    const auto[lat_2, lon_2] = y;
    constexpr long double R = 6'371'000;

    // Convert to radians
    const auto phi1 = lat_1 * M_PI / 180;
    const auto phi2 = lat_2 * M_PI / 180;
    const auto d_phi = (lat_2 - lat_1) * M_PI / 180;
    const auto d_lambda = (lon_2 - lon_1) * M_PI / 180;

    // Square of half the chord length between the objects
    const auto a = std::pow(std::sin(d_phi / 2), 2) +
                   std::cos(phi1) * std::cos(phi2) * std::pow(std::sin(d_lambda / 2), 2);
    // Angular distance in radians
    const auto c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
    return R * c;
}

/**
 * Determines the geographical center of a building consisting of ambient nodes.
 *
 * @param nodes List of nodes of an OSM way.
 * @return Geocenter described by a pair of latitude and longitude respectively.
 */
inline auto barycenter(const osmium::WayNodeList& nodes) -> Location {
    const auto lat = std::accumulate(nodes.cbegin(), nodes.cend(), static_cast<long double>(0),
                                     [](auto lhs, const auto& node) { return lhs + node.lat(); });
    const auto lon = std::accumulate(nodes.cbegin(), nodes.cend(), static_cast<long double>(0),
                                     [](auto lhs, const auto& node) { return lhs + node.lon(); });
    const auto num = nodes.size();

    return { lat / num, lon / num };
}

inline auto barycenter(const Locations& locations) -> Location {
    const auto
        lat = std::accumulate(locations.cbegin(), locations.cend(), static_cast<long double>(0),
                              [](auto lhs, const auto& node) { return lhs + node.first; });
    const auto
        lon = std::accumulate(locations.cbegin(), locations.cend(), static_cast<long double>(0),
                              [](auto lhs, const auto& node) { return lhs + node.second; });
    const auto num = locations.size();

    return { lat / num, lon / num };
}
} // namespace graph

#endif // GRAPHS_UTILS_HPP


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

#include <fcppt/math/dim/componentwise_equal.hpp>
#include <fcppt/math/dim/static.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_dim_componentwise_equal
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::dim::static_<
		float,
		2
	>
	f2_dim;

	float const epsilon(
		0.00001f
	);

	BOOST_CHECK(
		fcppt::math::dim::componentwise_equal(
			f2_dim(
				1.f,
				2.f
			),
			f2_dim(
				1.f,
				2.f
			),
			epsilon
		)
	);

	BOOST_CHECK(
		!fcppt::math::dim::componentwise_equal(
			f2_dim(
				1.f,
				2.f
			),
			f2_dim(
				1.1f,
				2.f
			),
			epsilon
		)
	);
}


// chapter 3

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

typedef mpl::vector_c<int, 1, 0, 0, 0> mass;
typedef mpl::vector_c<int, 0, 1, 0, 0> length;
typedef mpl::vector_c<int, 0, 0, 1, 0> time;
typedef mpl::vector_c<int, 0, 1, -2, 0> accel;
typedef mpl::vector_c<int, 1, 1, -2, 0> force;
typedef mpl::vector_c<int, 0, 0, 0, 0> scalar;

template <class T, class Dimension>
struct Quantity
{
	Quantity(T x) : m_value(x) {}
	template <class OtherDimension>
	Quantity( Quantity<T, OtherDimension> &rhs) : m_value(rhs.value())
	{
		BOOST_STATIC_ASSERT((
			mpl::equal< Dimension, OtherDimension >::type::value
			));
	}

	T m_value;
	T value() const { return m_value; }

	template<class OtherDimension>
	Quantity<T, Dimension>& operator=(Quantity<T, OtherDimension> &rhs)
	{
		BOOST_STATIC_ASSERT((
			mpl::equal< Dimension, OtherDimension >::type::value
			));

		if (this != &rhs)
		{
			m_value = rhs.value();
		}
		return *this;
	}

	Quantity<T, Dimension> operator+(const Quantity<T, Dimension> &rhs)
	{
		return Quantity<T, Dimension>(value() + rhs.value());
	}
	Quantity<T, Dimension> operator-(const Quantity<T, Dimension> &rhs)
	{
		return Quantity<T, Dimension>(value() - rhs.value());
	}

	struct minus_f
	{
		template <class T1, class T2>
		struct apply : mpl::minus<T1,T2> {};
	};

	template <class OtherDimension>
	Quantity<
		T,
		typename mpl::transform<Dimension, OtherDimension, mpl::plus<_1,_2> >::type
	>
		operator*(const Quantity<T, OtherDimension> &rhs)
	{
		typedef typename mpl::transform<Dimension, OtherDimension, mpl::plus<_1,_2> >::type dim;
		return Quantity<T, dim>(value() * rhs.value());
	}

	template <class OtherDimension>
	Quantity<T, typename mpl::transform<Dimension, OtherDimension, minus_f>::type >
		operator/(const Quantity<T, OtherDimension> &rhs)
	{
		typedef typename mpl::transform<Dimension, OtherDimension, minus_f>::type dim;
		return Quantity<T, dim>(value() / rhs.value());
	}

};

void main()
{
	Quantity<float, mass> m(3.f);
	Quantity<float, mass> m2(4.f);
	Quantity<float, length> l(5.f);
	Quantity<float, accel> a(5.f);
	Quantity<float, mass> m3 = m + m2;
	Quantity<float, mass> m4 = m - m2;
	Quantity<float, force> f = m * a;

	m4 = f/a;

	BOOST_STATIC_ASSERT((
		mpl::plus<
		mpl::int_<2>
		,mpl::int_<3>
		,mpl::int_<4>
		>::type::value == 9
		));

}