#include <tf/transform_datatypes.h>

#include <urdf_parser/urdf_parser.h>

#include <boost/assign.hpp>

#include <walrus_drive_controller/walrus_drive_controller.h>

static double euclideanOfVectors(const urdf::Vector3& vec1, const urdf::Vector3& vec2)
{
  return std::sqrt(std::pow(vec1.x-vec2.x,2) +
                   std::pow(vec1.y-vec2.y,2) +
                   std::pow(vec1.z-vec2.z,2));
}

namespace walrus_drive_controller{

WalrusDriveController::WalrusDriveController()
  : command_timeout_(0.5)

  , main_tread_separation_(1.0)
  , main_tread_ground_contact_length_(1.0)
  , tread_width_(1.0)
  , tread_driver_radius_(1.0)

  , base_frame_id_("base_link")
  , odom_frame_id_("odom")
{
}

bool WalrusDriveController::init(hardware_interface::VelocityJointInterface* hw,
				 ros::NodeHandle& root_nh,
				 ros::NodeHandle &controller_nh)
{
  // compute name to use for logging
  const std::string complete_ns = controller_nh.getNamespace();
  std::size_t id = complete_ns.find_last_of("/");
  name_ = complete_ns.substr(id + 1);

  // Get joint names
  std::string left_tread_name;
  std::string right_tread_name;
  controller_nh.param("left_tread_joint", left_tread_name, left_tread_name);
  controller_nh.param("right_tread_joint", right_tread_name, right_tread_name);

  // Tread specifications
  controller_nh.param("main_tread_separation", main_tread_separation_, main_tread_separation_);
  ROS_INFO_STREAM_NAMED(name_, "Main tread separation: " << main_tread_separation_);

  controller_nh.param("main_tread_ground_contact_length", main_tread_ground_contact_length_, main_tread_ground_contact_length_);
  ROS_INFO_STREAM_NAMED(name_, "Main tread ground contact length: " << main_tread_ground_contact_length_);

  controller_nh.param("tread_width", tread_width_, tread_width_);
  ROS_INFO_STREAM_NAMED(name_, "Tread width: " << tread_width_);

  controller_nh.param("tread_driver_radius", tread_driver_radius_, tread_driver_radius_);
  ROS_INFO_STREAM_NAMED(name_, "Tread driver radius: " << tread_driver_radius_);

  odometry_.setWheelParams(main_tread_separation_, tread_driver_radius_);

  // Rates and timeouts
  controller_nh.param("command_timeout", command_timeout_, command_timeout_);
  ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
                        << command_timeout_ << "s.");
  double publish_rate;
  controller_nh.param("publish_rate", publish_rate, 50.0);
  ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
                        << publish_rate << "Hz.");
  publish_period_ = ros::Duration(1.0 / publish_rate);

  // TF Frames
  controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
  ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);

  controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
  ROS_INFO_STREAM_NAMED(name_, "Odom frame_id set to " << odom_frame_id_);

  // Velocity and acceleration limits:
  controller_nh.param("linear/x/has_velocity_limits"    , limiter_lin_.has_velocity_limits    , limiter_lin_.has_velocity_limits    );
  controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
  controller_nh.param("linear/x/max_velocity"           , limiter_lin_.max_velocity           ,  limiter_lin_.max_velocity          );
  controller_nh.param("linear/x/min_velocity"           , limiter_lin_.min_velocity           , -limiter_lin_.max_velocity          );
  controller_nh.param("linear/x/max_acceleration"       , limiter_lin_.max_acceleration       ,  limiter_lin_.max_acceleration      );
  controller_nh.param("linear/x/min_acceleration"       , limiter_lin_.min_acceleration       , -limiter_lin_.max_acceleration      );

  controller_nh.param("angular/z/has_velocity_limits"    , limiter_ang_.has_velocity_limits    , limiter_ang_.has_velocity_limits    );
  controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
  controller_nh.param("angular/z/max_velocity"           , limiter_ang_.max_velocity           ,  limiter_ang_.max_velocity          );
  controller_nh.param("angular/z/min_velocity"           , limiter_ang_.min_velocity           , -limiter_ang_.max_velocity          );
  controller_nh.param("angular/z/max_acceleration"       , limiter_ang_.max_acceleration       ,  limiter_ang_.max_acceleration      );
  controller_nh.param("angular/z/min_acceleration"       , limiter_ang_.min_acceleration       , -limiter_ang_.max_acceleration      );

  setOdomPubFields(root_nh, controller_nh);

  left_tread_joint_ = hw->getHandle(left_tread_name);
  right_tread_joint_ = hw->getHandle(right_tread_name);

  cmd_vel_sub_ = controller_nh.subscribe("cmd_vel", 1, &WalrusDriveController::cmdVelCallback, this);
  tank_drive_sub_ = controller_nh.subscribe("tank_drive", 1, &WalrusDriveController::tankCallback, this);

  return true;
}

void WalrusDriveController::update(const ros::Time& time, const ros::Duration& period)
{

  double left_pos  = left_tread_joint_.getPosition();
  double right_pos = right_tread_joint_.getPosition();
  odometry_.update(left_pos, right_pos, time);

  // Publish odometry message
  if(last_state_publish_time_ + publish_period_ < time)
  {
    last_state_publish_time_ += publish_period_;
    // Compute and store orientation info
    const geometry_msgs::Quaternion orientation(
						  tf::createQuaternionMsgFromYaw(odometry_.getHeading()));

    // Populate odom message and publish
    if(odom_pub_->trylock())
    {
      odom_pub_->msg_.header.stamp = time;
      odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
      odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
      odom_pub_->msg_.pose.pose.orientation = orientation;
      odom_pub_->msg_.twist.twist.linear.x  = odometry_.getLinear();
      odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
      odom_pub_->unlockAndPublish();
    }

    // Publish tf /odom frame
    if (tf_odom_pub_->trylock())
    {
      geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
      odom_frame.header.stamp = time;
      odom_frame.transform.translation.x = odometry_.getX();
      odom_frame.transform.translation.y = odometry_.getY();
      odom_frame.transform.rotation = orientation;
      tf_odom_pub_->unlockAndPublish();
    }
  }

  // MOVE ROBOT
  // Retreive current velocity command and time step:
  geometry_msgs::TwistStamped cmd_vel_stamped = *(cmd_vel_buffer_.readFromRT());
  walrus_drive_controller::TankDriveCommandStamped tank_drive_stamped = *(tank_drive_buffer_.readFromRT());

  // twist command in newer
  if(cmd_vel_stamped.header.stamp > tank_drive_stamped.header.stamp) {
    geometry_msgs::Twist curr_cmd = cmd_vel_stamped.twist;
    const double dt = (time - cmd_vel_stamped.header.stamp).toSec();

    // Brake if cmd_vel has timeout:
    if (dt > command_timeout_) {
      brake();
    }
    else {
      // Limit velocities and accelerations:
      const double cmd_dt(period.toSec());
      limiter_lin_.limit(curr_cmd.linear.x, odometry_.getLinear(), cmd_dt);
      limiter_ang_.limit(curr_cmd.angular.z, odometry_.getAngular(), cmd_dt);

      const double vel_left  = (curr_cmd.linear.x - curr_cmd.angular.z * main_tread_separation_ / 2.0) / tread_driver_radius_;
      const double vel_right = (curr_cmd.linear.x + curr_cmd.angular.z * main_tread_separation_ / 2.0) / tread_driver_radius_;

      left_tread_joint_.setCommand(vel_left);
      right_tread_joint_.setCommand(vel_right);
    }
  }
  else {
    walrus_drive_controller::TankDriveCommand curr_cmd = tank_drive_stamped.command;
    const double dt = (time - tank_drive_stamped.header.stamp).toSec();

    if (dt > command_timeout_) {
      brake();
    }
    else {
      left_tread_joint_.setCommand(curr_cmd.left_speed/tread_driver_radius_);
      right_tread_joint_.setCommand(curr_cmd.right_speed/tread_driver_radius_);
    }
  }

}

void WalrusDriveController::starting(const ros::Time& time)
{
  brake();

  // Register starting time used to keep fixed rate
  last_state_publish_time_ = time;

  odometry_.init(time);
  geometry_msgs::TwistStamped twist;
  twist.header.stamp = ros::Time(0);
  twist.twist.linear.x = 0;
  twist.twist.angular.z = 0;
  cmd_vel_buffer_.initRT(twist);
  walrus_drive_controller::TankDriveCommandStamped tank_drive;
  tank_drive.header.stamp = ros::Time(0);
  tank_drive.command.left_speed = 0;
  tank_drive.command.right_speed = 0;
  tank_drive_buffer_.initRT(tank_drive);
}

void WalrusDriveController::stopping(const ros::Time& time)
{
  brake();
}

void WalrusDriveController::brake()
{
  left_tread_joint_.setCommand(0.0);
  right_tread_joint_.setCommand(0.0);
}

void WalrusDriveController::cmdVelCallback(const geometry_msgs::Twist& command)
{
  if(isRunning())
  {
    geometry_msgs::TwistStamped command_stamped;
    command_stamped.header.stamp = ros::Time::now();
    command_stamped.twist = command;
    cmd_vel_buffer_.writeFromNonRT(command_stamped);
  }
  else
  {
    ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
  }
}
void WalrusDriveController::tankCallback(const walrus_drive_controller::TankDriveCommand& command)
{
  if(isRunning())
  {
    walrus_drive_controller::TankDriveCommandStamped command_stamped;
    command_stamped.header.stamp = ros::Time::now();
    command_stamped.command = command;
    tank_drive_buffer_.writeFromNonRT(command_stamped);
  }
  else
  {
    ROS_ERROR_NAMED(name_, "Can't accept new commands. Controller is not running.");
  }
}

void WalrusDriveController::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
{
  // Get and check params for covariances
  XmlRpc::XmlRpcValue pose_cov_list;
  controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
  ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  ROS_ASSERT(pose_cov_list.size() == 6);
  for (int i = 0; i < pose_cov_list.size(); ++i)
    ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

  XmlRpc::XmlRpcValue twist_cov_list;
  controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
  ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
  ROS_ASSERT(twist_cov_list.size() == 6);
  for (int i = 0; i < twist_cov_list.size(); ++i)
    ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);

  // Setup odometry realtime publisher + odom message constant fields
  odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
  odom_pub_->msg_.header.frame_id = odom_frame_id_;
  odom_pub_->msg_.child_frame_id = base_frame_id_;
  odom_pub_->msg_.pose.pose.position.z = 0;
  odom_pub_->msg_.pose.covariance = boost::assign::list_of
    (static_cast<double>(pose_cov_list[0])) (0)  (0)  (0)  (0)  (0)
    (0)  (static_cast<double>(pose_cov_list[1])) (0)  (0)  (0)  (0)
    (0)  (0)  (static_cast<double>(pose_cov_list[2])) (0)  (0)  (0)
    (0)  (0)  (0)  (static_cast<double>(pose_cov_list[3])) (0)  (0)
    (0)  (0)  (0)  (0)  (static_cast<double>(pose_cov_list[4])) (0)
    (0)  (0)  (0)  (0)  (0)  (static_cast<double>(pose_cov_list[5]));
  odom_pub_->msg_.twist.twist.linear.y  = 0;
  odom_pub_->msg_.twist.twist.linear.z  = 0;
  odom_pub_->msg_.twist.twist.angular.x = 0;
  odom_pub_->msg_.twist.twist.angular.y = 0;
  odom_pub_->msg_.twist.covariance = boost::assign::list_of
    (static_cast<double>(twist_cov_list[0])) (0)  (0)  (0)  (0)  (0)
    (0)  (static_cast<double>(twist_cov_list[1])) (0)  (0)  (0)  (0)
    (0)  (0)  (static_cast<double>(twist_cov_list[2])) (0)  (0)  (0)
    (0)  (0)  (0)  (static_cast<double>(twist_cov_list[3])) (0)  (0)
    (0)  (0)  (0)  (0)  (static_cast<double>(twist_cov_list[4])) (0)
    (0)  (0)  (0)  (0)  (0)  (static_cast<double>(twist_cov_list[5]));
  tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
  tf_odom_pub_->msg_.transforms.resize(1);
  tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
  tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
  tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
}

} // namespace diff_drive_controller


#ifndef FOLLOW_TRAJECTORY
#define FOLLOW_TRAJECTORY

#include "rclcpp/rclcpp.hpp"
#include <pcl/point_types.h>
#include <signal.h>
#include <boost/foreach.hpp>
#include <math.h>
#include <stdio.h>
#include <cmath>
#include <iostream>
#include <fstream>
#include <cinematography_msgs/msg/multi_dof.hpp>
#include <cinematography_msgs/msg/multi_do_farray.hpp>
#include "nav_msgs/msg/odometry.hpp"
#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"

#define MARGIN 0.01     // Used as a margin of error to prevent the call to follow_trajectory from going over the node's period
#define DAMPING_FACTOR_X 0.05
#define DAMPING_FACTOR_Y 0.05
#define DAMPING_FACTOR_Z 0.15
#define likely(x)      __builtin_expect(!!(x), 1)
#define unlikely(x)    __builtin_expect(!!(x), 0)

#define MAX_YAW_RATE    90

using namespace std;
using std::placeholders::_1;

struct multiDOFpoint {
    double x, y, z;
    double vx, vy, vz;
    double ax, ay, az;
    double yaw;
    double duration;
};
typedef std::deque<multiDOFpoint> trajectory_t;

class FollowTrajectory : public rclcpp::Node {
private:
    trajectory_t normal_traj;
    trajectory_t rev_normal_traj;
    trajectory_t slam_loss_traj;
    trajectory_t panic_traj;
    float g_v_max = 6.0;

    float g_max_yaw_rate = 90;
    float g_max_yaw_rate_during_flight = 90;

    bool g_trajectory_done;

    bool should_panic = false;
    bool following_traj = false;  //decides when the first planning has occured
    // geometry_msgs::msg::Vector3 panic_velocity;

    int rviz_id = 0;

    int traj_id = 0;

    msr::airlib::MultirotorRpcLibClient* airsim_client;
    std::chrono::system_clock::time_point sleep_time; //todo

    rclcpp::Node::SharedPtr node;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr rviz_vel_pub;
    rclcpp::Subscription<cinematography_msgs::msg::MultiDOFarray>::SharedPtr trajectory_follower_sub;
    rclcpp::TimerBase::SharedPtr timer_follow_traj;

    void callback_trajectory(const cinematography_msgs::msg::MultiDOFarray::SharedPtr msg);

    void print_rviz_vel(double x, double y, double z, double vx, double vy, double vz);

    void follow_traj();

public:
    FollowTrajectory();

    ~FollowTrajectory() {
        delete airsim_client;
    }
};

#endif


#include <math.h>
#include <iostream>
#include <vector>
#include <boost/math/common_factor.hpp>
#include <gecode/gist.hh>

using namespace std;

class ModelExec{
public:
	template <class T>
	void ExecModel (T a, int mode)
	{
		Gist::Print<T> p("Print solution");
		Gist::Options o;
		o.inspect.click(&p);
		Gist::bab(model,o);
		delete model;
		break;
		return;
	}

};


#include "relativeeyelidlearner.h"

#include <boost/lexical_cast.hpp>

using namespace std;

RelativeEyeLidLearner::RelativeEyeLidLearner(TrainingParameters params) : AbstractLearner(params)
{
    if (!trainParams.pca_epsilon.is_initialized()) {
        use_pca = true;
        trainParams.pca_epsilon = 0.85;
    }
}

RelativeEyeLidLearner::~RelativeEyeLidLearner()
{

}

void RelativeEyeLidLearner::loadClassifier(const string &filename)
{
    _loadClassifier(filename, learned_function);
}

boost::optional<dlib::matrix<double,0,1>> RelativeEyeLidLearner::getFeatureVector(GazeHyp& ghyp) {
    auto result = boost::optional<dlib::matrix<double,0,1>>();
    if (ghyp.faceFeatures.size() && ghyp.lidFeatures.size()) {
        switch (trainParams.featureSet.get_value_or(FeatureSetConfig::ALL)) {
        case FeatureSetConfig::ALL:
        case FeatureSetConfig::HOGPOS:
            result = dlib::join_cols(dlib::rowm(ghyp.faceFeatures, dlib::range(0, ghyp.faceFeatures.nr()-5)), ghyp.eyeHogFeatures);
            break;
        case FeatureSetConfig::HOG:
            result = ghyp.eyeHogFeatures;
            break;
        case FeatureSetConfig::POSITIONAL:
            result = dlib::rowm(ghyp.faceFeatures, dlib::range(0, ghyp.faceFeatures.nr()-5));
            break;
        default:
            throw std::runtime_error("feature-set not implemented");
        }
    }
    return result;
}

void RelativeEyeLidLearner::classify(GazeHyp &ghyp)
{
    _classify(ghyp, learned_function, ghyp.eyeLidClassification);
}

void RelativeEyeLidLearner::train(const string &outfilename)
{
    dlib::svr_trainer<kernel_type> trainer;
    _train(outfilename, learned_function, trainer, true);
}

void RelativeEyeLidLearner::visualize(GazeHyp &ghyp)
{
    if (!ghyp.eyeLidClassification.is_initialized() || !learned_function.basis_vectors.size()) return;
    auto eoclass = ghyp.eyeLidClassification.get();
    if (!std::isfinite(eoclass)) return;
    int color = ghyp.isLidClosed.get_value_or(false) ? 255 : 80;
    cv::rectangle(ghyp.eyePatch, cv::Rect(cv::Point(eoclass*ghyp.eyePatch.cols), cv::Size(2, 2)),
                  cv::Scalar(255, color-50, color-50), -1, 'A');
    cv::Rect r = ghyp.pupils.faceRect();
    cv::rectangle(ghyp.parentHyp.frame, cv::Rect(cv::Point(r.x+r.width, r.y+eoclass*r.height), cv::Size(5, 5)),
                  cv::Scalar(255, color-50, color-50), -1, 'A');
    if (ghyp.isLidClosed.get_value_or(false)) {
        cv::line(ghyp.parentHyp.frame, r.tl()+cv::Point(r.width/5, r.height/5),
                 r.br()-cv::Point(r.width/5, r.height/5), cv::Scalar(0, 0, 255), 2, 'A');
        cv::line(ghyp.parentHyp.frame, r.tl()+cv::Point(r.width-r.width/5, r.height/5),
                 r.tl()+cv::Point(r.width/5, r.height-r.height/5), cv::Scalar(0, 0, 255), 2, 'A');
    }
}

string RelativeEyeLidLearner::getId()
{
    return "RelativeEyeLidLearner";
}


#include "PurificationDecorator.hpp"
#include "IRProvider.hpp"
#include "InstructionIterator.hpp"
#include "PauliOperator.hpp"
#include "XACC.hpp"
#include <Eigen/Dense>
#include "xacc_service.hpp"

using namespace xacc::quantum;

namespace xacc {
namespace vqe {
void PurificationDecorator::execute(std::shared_ptr<AcceleratorBuffer> buffer,
                                    const std::shared_ptr<Function> function) {

  if (!decoratedAccelerator) {
    xacc::error("Null Decorated Accelerator Error");
  }

  return;
}

std::vector<std::shared_ptr<AcceleratorBuffer>> PurificationDecorator::execute(
    std::shared_ptr<AcceleratorBuffer> buffer,
    const std::vector<std::shared_ptr<Function>> functions) {

  std::vector<std::shared_ptr<AcceleratorBuffer>> buffers;

  if (!decoratedAccelerator) {
    xacc::error("PurificationDecorator - Null Decorated Accelerator Error");
  }

  // Here I expect the ansatz to be functions[0]->getInstruction(0);
  auto ansatz =
      std::dynamic_pointer_cast<Function>(functions[0]->getInstruction(0));

  if (!ansatz)
    xacc::error("ANSATZ IS NULL");

  // Generate all nQubit Pauli Strings
  std::vector<std::string> XYZ;
  std::set<std::string> temp, PauliStrings;
  std::vector<PauliOperator> Paulis;

  for (int i = 0; i < buffer->size(); i++)
    XYZ.push_back("X" + std::to_string(i));
  for (int i = 0; i < buffer->size(); i++)
    XYZ.push_back("Y" + std::to_string(i));
  for (int i = 0; i < buffer->size(); i++)
    XYZ.push_back("Z" + std::to_string(i));

  std::function<void(std::vector<std::string> &, std::string, const int,
                     const int)>
      y;
  y = [&](std::vector<std::string> &set, std::string prefix, const int n,
          const int k) {
    if (k == 0) {
      PauliOperator op;
      op.fromString(prefix);
      PauliStrings.insert(op.toString());
      return;
    }

    for (int i = 0; i < n; i++) {
      auto newPrefix = prefix + " " + set[i];
      y(set, newPrefix, n, k - 1);
    }
  };

  auto x = [&](std::vector<std::string> &set, const int k) {
    auto n = set.size();
    y(set, std::string(""), n, k);
  };

  // Get all Permutations of XYZ
  x(XYZ, buffer->size());

  PauliOperator all;
  std::map<std::string, PauliOperator> opMap;
  for (auto &s : PauliStrings) {
    PauliOperator op;
    op.fromString(s);
    Paulis.push_back(op);
    all += op;
    opMap.insert({op.getTerms().begin()->second.id(), op});
  }

  for (auto &kv : opMap)
    std::cout << kv.first << ", " << kv.second.toString() << "\n";

  std::cout << all.toString() << "\n";
  std::cout << Paulis.size() << " operators to measure\n";

  auto kernels = all.toXACCIR()->getKernels();
  for (auto &k : kernels) {
    k->insertInstruction(0, ansatz);
  }

//   std::cout << "EXECUTING\n";
  buffers = decoratedAccelerator->execute(buffer, kernels);

  std::size_t dim = 1;
  std::size_t two = 2;
  for (int i = 0; i < buffer->size(); i++)
    dim *= two;

  Eigen::MatrixXcd rho(dim, dim);
  rho.setZero();

  std::map<std::string, std::shared_ptr<AcceleratorBuffer>> bufMap;
  for (auto &b : buffers) {
    auto id = b->name();
    bufMap.insert({id,b});
    auto op = opMap[id];

    auto data = op.toDenseMatrix(buffer->size()).data();
    Eigen::MatrixXcd p = Eigen::Map<Eigen::MatrixXcd>(data, dim,dim);
    rho += b->getExpectationValueZ() * p;
  }

  // Add the Identity term Tr(I rho) * I = <I> * I
  rho += Eigen::MatrixXcd::Identity(dim, dim);

//   std::cout << "RHO:\n" << rho << "\n";
//   std::cout << rho.trace() << "\n";

  Eigen::MatrixXcd rhosq = rho * rho;
  Eigen::MatrixXcd diff = rhosq - rho;
  Eigen::MatrixXcd diffsq = diff * diff;

  int counter = 0;
  auto tr = std::real(diffsq.trace());
  while (tr > 1e-4) {
    rho = 3. * rhosq - 2. * rhosq * rho;
    rho /= rho.trace();

    rhosq = rho * rho;
    diff = rhosq - rho;
    diffsq = diff * diff;
    tr = std::real(diffsq.trace());
    // std::cout << counter << ", TRACE: " << tr << "\n";
    counter++;
    if (counter > 100)
      break;
  }

//   std::cout << rho.trace() << "\n" << rho << "\n";

  // new E = Tr(H*rho)
  // so get H
  PauliOperator HOp;
  auto ir = xacc::getService<xacc::IRProvider>("gate")->createIR();
  for (auto &f : functions) {
    ir->addKernel(f);
  }
  HOp.fromXACCIR(ir);

//   std::cout << "MADE IT HERE\n";
//   std::cout << HOp.toString() << "\n";

  auto identityCoeff = mpark::get<double>(buffer->getInformation("identity-coeff"));
  xacc::info(std::to_string(identityCoeff));
  auto ID = -1 * identityCoeff * Eigen::MatrixXcd::Identity(dim,dim);
  auto data = HOp.toDenseMatrix(buffer->size()).data();
  Eigen::MatrixXcd H = Eigen::Map<Eigen::MatrixXcd>(data, dim,dim) - ID;

//   std::cout << "CONJ:\n" << (rho - rho.conjugate()) << "\n";
//   std::cout << "IDEMP:\n" << (rho*rho - rho) << "\n";
//   std::cout << "H:\n" << H << "\n";
//   std::cout << "HI\n";
  std::cout << "Energy: " << std::real((rho * H).trace()) << "\n";

  // Need to take new rho and compute <P> = Tr(P rho) for each of our
  // input functions

  std::vector<std::shared_ptr<AcceleratorBuffer>> retBuffers;
  for (auto& f : functions) {
//       if (f->name() == "I") {
//           continue;
//       }
//     auto tmp = xacc::getService<xacc::IRProvider>("gate")->createIR();
//     tmp->addKernel(f);

//     PauliOperator o;
//     o.fromXACCIR(tmp);

//     auto mat = o.toDenseMatrix(buffer->size());
//     auto expVal = std::real((rho * mat).trace());

    if (bufMap.count(f->name())) {
    auto b = bufMap[f->name()];
    b->addExtraInfo("purified-energy",ExtraInfo(std::real((rho*H).trace())));
//     b->addExtraInfo("exp-val-z", ExtraInfo(expVal));
    retBuffers.push_back(b);
    }
  }

  return retBuffers;
}

} // namespace vqe
} // namespace xacc


#include "Clusterable.h"
#include "cluster.h"
#include "common.h"
#include "molecule.h"

#include <boost/core/demangle.hpp>
#include <fstream>
#include <iostream>

namespace clustering {
template <>
std::vector<Atom> collapse<Atom, Cluster<Atom>>(Cluster<Atom> const& c) {
  return c.collapse();
}

auto center(Cluster<Atom> const& c) { return c.center(); }
}  // namespace clustering

auto atom_to_clusterables(std::vector<Atom> const& vs) {
  std::vector<clustering::Clusterable<Atom>> out;
  for (auto const& v : vs) {
    out.emplace_back(v);
  }
  return out;
}

int main(int argc, char** argv) {
  std::string filename = (argc > 1) ? argv[1] : "";
  if (filename.empty()) {
    std::cout << "Didn't pass in an xyz file\n";
    return 1;
  }

  std::ifstream is(filename);
  std::vector<Atom> atoms;
  if (filename.rfind(".xyz") != std::string::npos)
    atoms = libint2::read_dotxyz(is);
  else
    throw "only .xyz files are accepted";

  auto natoms = atoms.size();
  std::cout << "Number of atoms: " << natoms << std::endl;

  auto clusterables = atom_to_clusterables(atoms);

  auto n_small_clusters = std::max(int(natoms) / 2, 1);
  auto magic_seed = 42;
  clustering::Kmeans kmeans_small(n_small_clusters, magic_seed);

  auto small_clusters = kmeans_small(clusterables);

  auto n = 1;
  for (auto const& c : small_clusters) {
    std::cout << "Cluster " << n << " has atoms:\n";
    for (auto atom : c.collapse()) {
      std::cout << "\t" << center(atom).transpose() << "\n";
    }
    std::cout << std::flush;
    ++n;
  }

  std::vector<clustering::Clusterable<Atom>> meta_clusters;
  for (auto const& cluster : small_clusters) {
    meta_clusters.push_back(cluster);
  }

  if (meta_clusters.size() > 10) {
    std::cout << "\n\nClustering our small clusters\n";
    clustering::Kmeans kmeans_big(4, magic_seed);
    auto clusters_of_clusters = kmeans_big(meta_clusters);

    auto n = 1;
    for (auto const& c : clusters_of_clusters) {
      std::cout << "Cluster " << n << " has atoms:\n";
      for (auto atom : c.collapse()) {
        std::cout << "\t" << center(atom).transpose() << "\n";
      }
      std::cout << std::flush;
      ++n;
    }
  }

  return 0;
}


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

#ifndef BOOST_HISTOGRAM_AXIS_REGULAR_HPP
#define BOOST_HISTOGRAM_AXIS_REGULAR_HPP

#include <boost/assert.hpp>
#include <boost/histogram/axis/interval_view.hpp>
#include <boost/histogram/axis/iterator.hpp>
#include <boost/histogram/axis/option.hpp>
#include <boost/histogram/detail/compressed_pair.hpp>
#include <boost/histogram/detail/meta.hpp>
#include <boost/histogram/fwd.hpp>
#include <boost/mp11/utility.hpp>
#include <boost/throw_exception.hpp>
#include <cmath>
#include <limits>
#include <stdexcept>
#include <type_traits>
#include <utility>

namespace boost {
namespace histogram {
namespace axis {

namespace transform {

/// Identity transform for equidistant bins.
struct id {
  /// Pass-through.
  template <typename T>
  static T forward(T&& x) noexcept {
    return std::forward<T>(x);
  }

  /// Pass-through.
  template <typename T>
  static T inverse(T&& x) noexcept {
    return std::forward<T>(x);
  }
};

/// Log transform for equidistant bins in log-space.
struct log {
  /// Returns log(x) of external value x.
  template <typename T>
  static T forward(T x) {
    return std::log(x);
  }

  /// Returns exp(x) for internal value x.
  template <typename T>
  static T inverse(T x) {
    return std::exp(x);
  }
};

/// Sqrt transform for equidistant bins in sqrt-space.
struct sqrt {
  /// Returns sqrt(x) of external value x.
  template <typename T>
  static T forward(T x) {
    return std::sqrt(x);
  }

  /// Returns x^2 of internal value x.
  template <typename T>
  static T inverse(T x) {
    return x * x;
  }
};

/// Pow transform for equidistant bins in pow-space.
struct pow {
  double power = 1; /**< power index */

  /// Make transform with index p.
  explicit pow(double p) : power(p) {}
  pow() = default;

  /// Returns pow(x, power) of external value x.
  template <typename T>
  auto forward(T x) const {
    return std::pow(x, power);
  }

  /// Returns pow(x, 1/power) of external value x.
  template <typename T>
  auto inverse(T x) const {
    return std::pow(x, 1.0 / power);
  }

  bool operator==(const pow& o) const noexcept { return power == o.power; }
};

} // namespace transform

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
// Type envelope to mark value as step size
template <typename T>
struct step_type {
  T value;
};
#endif

/**
  Helper function to mark argument as step size.
 */
template <typename T>
auto step(T&& t) {
  return step_type<T&&>{std::forward<T>(t)};
}

/**
  Axis for equidistant intervals on the real line.

  The most common binning strategy. Very fast. Binning is a O(1) operation.

  @tparam Value input value type, must be floating point.
  @tparam Transform builtin or user-defined transform type.
  @tparam MetaData type to store meta data.
  @tparam Options see boost::histogram::axis::option (all values allowed).
 */
template <class Value, class Transform, class MetaData, class Options>
class regular : public iterator_mixin<regular<Value, Transform, MetaData, Options>>,
                protected detail::replace_default<Transform, transform::id> {
  using value_type = Value;
  using transform_type = detail::replace_default<Transform, transform::id>;
  using metadata_type = detail::replace_default<MetaData, std::string>;
  using options_type =
      detail::replace_default<Options, decltype(option::underflow | option::overflow)>;

  using unit_type = detail::get_unit_type<value_type>;
  using internal_value_type = detail::get_scale_type<value_type>;
  static_assert(std::is_floating_point<internal_value_type>::value,
                "variable axis requires floating point type");

public:
  constexpr regular() = default;

  /** Construct n bins over real transformed range [start, stop).
   *
   * @param trans    transform instance to use.
   * @param n        number of bins.
   * @param start    low edge of first bin.
   * @param stop     high edge of last bin.
   * @param meta     description of the axis (optional).
   */
  regular(transform_type trans, unsigned n, value_type start, value_type stop,
          metadata_type meta = {})
      : transform_type(std::move(trans))
      , size_meta_(static_cast<index_type>(n), std::move(meta))
      , min_(this->forward(detail::get_scale(start)))
      , delta_(this->forward(detail::get_scale(stop)) - min_) {
    if (size() == 0) BOOST_THROW_EXCEPTION(std::invalid_argument("bins > 0 required"));
    if (!std::isfinite(min_) || !std::isfinite(delta_))
      BOOST_THROW_EXCEPTION(
          std::invalid_argument("forward transform of start or stop invalid"));
    if (delta_ == 0)
      BOOST_THROW_EXCEPTION(std::invalid_argument("range of axis is zero"));
  }

  /** Construct n bins over real range [start, stop).
   *
   * @param n        number of bins.
   * @param start    low edge of first bin.
   * @param stop     high edge of last bin.
   * @param meta     description of the axis (optional).
   */
  regular(unsigned n, value_type start, value_type stop, metadata_type meta = {})
      : regular({}, n, start, stop, std::move(meta)) {}

  /** Construct bins with the given step size over real transformed range
   * [start, stop).
   *
   * @param trans   transform instance to use.
   * @param step    width of a single bin.
   * @param start   low edge of first bin.
   * @param stop    upper limit of high edge of last bin (see below).
   * @param meta    description of the axis (optional).
   *
   * The axis computes the number of bins as n = abs(stop - start) / step,
   * rounded down. This means that stop is an upper limit to the actual value
   * (start + n * step).
   */
  template <class T>
  regular(transform_type trans, const step_type<T>& step, value_type start,
          value_type stop, metadata_type meta = {})
      : regular(trans, static_cast<index_type>(std::abs(stop - start) / step.value),
                start,
                start + static_cast<index_type>(std::abs(stop - start) / step.value) *
                            step.value,
                std::move(meta)) {}

  /** Construct bins with the given step size over real range [start, stop).
   *
   * @param step    width of a single bin.
   * @param start   low edge of first bin.
   * @param stop    upper limit of high edge of last bin (see below).
   * @param meta    description of the axis (optional).
   *
   * The axis computes the number of bins as n = abs(stop - start) / step,
   * rounded down. This means that stop is an upper limit to the actual value
   * (start + n * step).
   */
  template <class T>
  regular(const step_type<T>& step, value_type start, value_type stop,
          metadata_type meta = {})
      : regular({}, step, start, stop, std::move(meta)) {}

  /// Constructor used by algorithm::reduce to shrink and rebin (not for users).
  regular(const regular& src, index_type begin, index_type end, unsigned merge)
      : regular(src.transform(), (end - begin) / merge, src.value(begin), src.value(end),
                src.metadata()) {
    BOOST_ASSERT((end - begin) % merge == 0);
    if (options_type::test(option::circular) && !(begin == 0 && end == src.size()))
      BOOST_THROW_EXCEPTION(std::invalid_argument("cannot shrink circular axis"));
  }

  /// Return instance of the transform type.
  const transform_type& transform() const noexcept { return *this; }

  /// Return index for value argument.
  index_type index(value_type x) const noexcept {
    // Runs in hot loop, please measure impact of changes
    auto z = (this->forward(x / unit_type{}) - min_) / delta_;
    if (options_type::test(option::circular)) {
      if (std::isfinite(z)) {
        z -= std::floor(z);
        return static_cast<index_type>(z * size());
      }
    } else {
      if (z < 1) {
        if (z >= 0)
          return static_cast<index_type>(z * size());
        else
          return -1;
      }
    }
    return size(); // also returned if x is NaN
  }

  /// Returns index and shift (if axis has grown) for the passed argument.
  auto update(value_type x) noexcept {
    BOOST_ASSERT(options_type::test(option::growth));
    const auto z = (this->forward(x / unit_type{}) - min_) / delta_;
    if (z < 1) { // don't use i here!
      if (z >= 0) {
        const auto i = static_cast<axis::index_type>(z * size());
        return std::make_pair(i, 0);
      }
      if (z != -std::numeric_limits<internal_value_type>::infinity()) {
        const auto stop = min_ + delta_;
        const auto i = static_cast<axis::index_type>(std::floor(z * size()));
        min_ += i * (delta_ / size());
        delta_ = stop - min_;
        size_meta_.first() -= i;
        return std::make_pair(0, -i);
      }
      // z is -infinity
      return std::make_pair(-1, 0);
    }
    // z either beyond range, infinite, or NaN
    if (z < std::numeric_limits<internal_value_type>::infinity()) {
      const auto i = static_cast<axis::index_type>(z * size());
      const auto n = i - size() + 1;
      delta_ /= size();
      delta_ *= size() + n;
      size_meta_.first() += n;
      return std::make_pair(i, -n);
    }
    // z either infinite or NaN
    return std::make_pair(size(), 0);
  }

  /// Return value for fractional index argument.
  value_type value(real_index_type i) const noexcept {
    auto z = i / size();
    if (!options_type::test(option::circular) && z < 0.0)
      z = -std::numeric_limits<internal_value_type>::infinity() * delta_;
    else if (options_type::test(option::circular) || z <= 1.0)
      z = (1.0 - z) * min_ + z * (min_ + delta_);
    else {
      z = std::numeric_limits<internal_value_type>::infinity() * delta_;
    }
    return static_cast<value_type>(this->inverse(z) * unit_type());
  }

  /// Return bin for index argument.
  decltype(auto) bin(index_type idx) const noexcept {
    return interval_view<regular>(*this, idx);
  }

  /// Returns the number of bins, without over- or underflow.
  index_type size() const noexcept { return size_meta_.first(); }
  /// Returns the options.
  static constexpr unsigned options() noexcept { return options_type::value; }
  /// Returns reference to metadata.
  metadata_type& metadata() noexcept { return size_meta_.second(); }
  /// Returns reference to const metadata.
  const metadata_type& metadata() const noexcept { return size_meta_.second(); }

  template <class V, class T, class M, class O>
  bool operator==(const regular<V, T, M, O>& o) const noexcept {
    return detail::relaxed_equal(transform(), o.transform()) && size() == o.size() &&
           detail::relaxed_equal(metadata(), o.metadata()) && min_ == o.min_ &&
           delta_ == o.delta_;
  }
  template <class V, class T, class M, class O>
  bool operator!=(const regular<V, T, M, O>& o) const noexcept {
    return !operator==(o);
  }

  template <class Archive>
  void serialize(Archive&, unsigned);

private:
  detail::compressed_pair<index_type, metadata_type> size_meta_{0};
  internal_value_type min_{0}, delta_{1};

  template <class V, class T, class M, class O>
  friend class regular;
};

#if __cpp_deduction_guides >= 201606

template <class T>
regular(unsigned, T, T)->regular<detail::convert_integer<T, double>>;

template <class T>
regular(unsigned, T, T, const char*)->regular<detail::convert_integer<T, double>>;

template <class T, class M>
regular(unsigned, T, T, M)->regular<detail::convert_integer<T, double>, transform::id, M>;

template <class Tr, class T>
regular(Tr, unsigned, T, T)->regular<detail::convert_integer<T, double>, Tr>;

template <class Tr, class T>
regular(Tr, unsigned, T, T, const char*)->regular<detail::convert_integer<T, double>, Tr>;

template <class Tr, class T, class M>
regular(Tr, unsigned, T, T, M)->regular<detail::convert_integer<T, double>, Tr, M>;

#endif

template <class Value = double, class MetaData = use_default, class Options = use_default>
using circular = regular<Value, transform::id, MetaData,
                         decltype(detail::replace_default<Options, option::overflow_t>{} |
                                  option::circular)>;

} // namespace axis
} // namespace histogram
} // namespace boost

#endif


#ifndef EIGEN_TYPEDEFS_HPP
#define EIGEN_TYPEDEFS_HPP

#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <map>

namespace EigenHelpers {
////////////////////////////////////////////////////////////////////////////
// Typedefs for aligned STL containers using Eigen types
////////////////////////////////////////////////////////////////////////////

typedef std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> VectorVector2f;
typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VectorVector2d;
typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> VectorVector3f;
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VectorVector3d;
typedef std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> VectorVector4f;
typedef std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> VectorVector4d;
typedef std::vector<Eigen::Quaternionf, Eigen::aligned_allocator<Eigen::Quaternionf>> VectorQuaternionf;
typedef std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> VectorQuaterniond;
typedef std::vector<Eigen::Isometry3f, Eigen::aligned_allocator<Eigen::Isometry3f>> VectorIsometry3f;
typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> VectorIsometry3d;
typedef std::vector<Eigen::Affine3f, Eigen::aligned_allocator<Eigen::Affine3f>> VectorAffine3f;
typedef std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d>> VectorAffine3d;
typedef std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> VectorMatrix4f;
typedef std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> VectorMatrix4d;
typedef std::vector<Eigen::Translation3f, Eigen::aligned_allocator<Eigen::Translation3f>> VectorTranslation3f;
typedef std::vector<Eigen::Translation3d, Eigen::aligned_allocator<Eigen::Translation3d>> VectorTranslation3d;
typedef std::map<std::string, Eigen::Vector2f, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector2f>>>
    MapStringVector2f;
typedef std::map<std::string, Eigen::Vector2d, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector2d>>>
    MapStringVector2d;
typedef std::map<std::string, Eigen::Vector3f, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3f>>>
    MapStringVector3f;
typedef std::map<std::string, Eigen::Vector3d, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3d>>>
    MapStringVector3d;
typedef std::map<std::string, Eigen::Vector4f, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4f>>>
    MapStringVector4f;
typedef std::map<std::string, Eigen::Vector4d, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4d>>>
    MapStringVector4d;
typedef std::map<std::string, Eigen::Quaternionf, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaternionf>>>
    MapStringQuaternionf;
typedef std::map<std::string, Eigen::Quaterniond, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaterniond>>>
    MapStringQuaterniond;
typedef std::map<std::string, Eigen::Isometry3f, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3f>>>
    MapStringIsometry3f;
typedef std::map<std::string, Eigen::Isometry3d, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d>>>
    MapStringIsometry3d;
typedef std::map<std::string, Eigen::Affine3f, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3f>>>
    MapStringAffine3f;
typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
                 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d>>>
    MapStringAffine3d;
}  // namespace EigenHelpers

#endif


#include <rushhour_results_seriesgenerator.h>
#include <rushhour_results_rangegroup.h>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <stdexcept>

namespace rushhour {
namespace results {

SeriesGenerator::SeriesGenerator(const std::vector<Result>& results)
: results_(results) {}

void SeriesGenerator::latency_vs_time(std::size_t bucket_count) {
  if (bucket_count == 0) {
    throw std::out_of_range("invalid number of buckets");
  }

  using accumulator_t = boost::accumulators::accumulator_set<double,
    boost::accumulators::stats<boost::accumulators::tag::mean>>;

  // find min/max times
  double min = std::numeric_limits<double>::max();
  double max = std::numeric_limits<double>::min();
  for (auto& r : results_) {
    double value = r.started().time_since_epoch().count();
    if (value < min) {
      min = value;
    }
    if (value > max) {
      max = value;
    }
  }

  RangeGroup<accumulator_t> buckets(bucket_count, min, max);

  for (auto& r : results_) {
    double index = r.started().time_since_epoch().count();
    buckets.at(index)(r.elapsed().count());
  }

}

}  // namespace results
}  // namespace rushhour


/*
 * scan_varying_helpers.cc
 *
 *  Copyright (C) 2014 STFC Rutherford Appleton Laboratory, UK.
 *
 *  Author: David Waterman
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/spot_prediction/scan_varying_helpers.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  void export_solve_quad() {
    def("solve_quad", reeke_detail::solve_quad, (arg("a"), arg("b"), arg("c")));
  }

}}}  // namespace dials::algorithms::boost_python


#include <puppet/compiler/evaluation/operators/binary/minus.hpp>
#include <puppet/compiler/evaluation/operators/binary/call_context.hpp>
#include <puppet/compiler/exceptions.hpp>
#include <boost/format.hpp>
#include <limits>
#include <cfenv>

using namespace std;
using namespace puppet::runtime;

namespace puppet { namespace compiler { namespace evaluation { namespace operators { namespace binary {

    static values::value subtract(call_context& context, int64_t left, int64_t right)
    {
        auto& evaluation_context = context.context();

        if (right > 0) {
            if (left < numeric_limits<int64_t>::min() + right) {
                throw evaluation_exception(
                    (boost::format("subtraction of %1% and %2% results in an arithmetic underflow.") %
                     left %
                     right
                    ).str(),
                    context.right_context(),
                    evaluation_context.backtrace()
                );
            }
        } else {
            if (left > numeric_limits<int64_t>::max() + right) {
                throw evaluation_exception(
                    (boost::format("subtraction of %1% and %2% results in an arithmetic overflow.") %
                     left %
                     right
                    ).str(),
                    context.right_context(),
                    evaluation_context.backtrace()
                );
            }
        }
        return left - right;
    }

    static values::value subtract(call_context& context, double left, double right)
    {
        auto& evaluation_context = context.context();

        feclearexcept(FE_OVERFLOW | FE_UNDERFLOW);
        double result = left - right;
        if (fetestexcept(FE_OVERFLOW)) {
            throw evaluation_exception(
                (boost::format("subtraction of %1% and %2% results in an arithmetic overflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        } else if (fetestexcept(FE_UNDERFLOW)) {
            throw evaluation_exception(
                (boost::format("subtraction of %1% and %2% results in an arithmetic underflow.") %
                 left %
                 right
                ).str(),
                context.right_context(),
                evaluation_context.backtrace()
            );
        }
        return result;
    }

    static values::value subtract(call_context& context, values::value const& left, values::value const& right)
    {
        if (left.as<int64_t>() && right.as<int64_t>()) {
            return subtract(context, left.require<int64_t>(), right.require<int64_t>());
        }
        if (left.as<int64_t>()) {
            return subtract(context, static_cast<double>(left.require<int64_t>()), right.require<double>());
        }
        if (right.as<int64_t>()) {
            return subtract(context, left.require<double>(), static_cast<double>(right.require<int64_t>()));
        }
        return subtract(context, left.require<double>(), right.require<double>());
    }

    // Declared in divide.cc
    values::value arithmetic_string_conversion(call_context& context, bool left = true);

    descriptor minus::create_descriptor()
    {
        binary::descriptor descriptor{ ast::binary_operator::minus };

        // The order of these overloads is important (most specific to least specific)
        descriptor.add("Numeric", "Numeric", [](call_context& context) {
            return subtract(context, context.left(), context.right());
        });
        descriptor.add("String", "Numeric", [](call_context& context) {
            return subtract(context, arithmetic_string_conversion(context), context.right());
        });
        descriptor.add("Numeric", "String", [](call_context& context) {
            return subtract(context, context.left(), arithmetic_string_conversion(context, false));
        });
        descriptor.add("String", "String", [](call_context& context) {
            return subtract(context, arithmetic_string_conversion(context), arithmetic_string_conversion(context, false));
        });
        descriptor.add("Array[Any]", "Array[Any]", [](call_context& context) {
            auto result = context.left().require<values::array>();
            auto& right = context.right().require<values::array>();

            // Remove any elements in left that are also in right
            result.erase(remove_if(result.begin(), result.end(), [&](values::value const& v) {
                return find_if(right.begin(), right.end(), [&](values::value const& v2) { return v == v2; }) != right.end();
            }), result.end());
            return result;
        });
        descriptor.add("Array[Any]", "Hash[Any, Any]", [](call_context& context) {
            auto result = context.left().require<values::array>();
            auto& right = context.right().require<values::hash>();

            // Remove any [K,V] elements in left that are key and value in right
            result.erase(remove_if(result.begin(), result.end(), [&](values::value const& v) {
                auto ptr = v.as<values::array>();
                if (!ptr || ptr->size() != 2) {
                    return false;
                }
                auto value = right.get((*ptr)[0]);
                return value && (*ptr)[1] == *value;
            }), result.end());
            return result;
        });
        descriptor.add("Array[Any]", "Any", [](call_context& context) {
            auto result = context.left().require<values::array>();
            auto& right = context.right();

            // Remove any elements in left that are equal to right
            result.erase(remove_if(result.begin(), result.end(), [&](values::value const& v) { return v == right; }), result.end());
            return result;
        });
        descriptor.add("Hash[Any, Any]", "Hash[Any, Any]", [](call_context& context) {
            auto result = context.left().require<values::hash>();
            auto& right = context.right().require<values::hash>();

            // Remove any elements in left that have keys in right
            for (auto const& kvp : right) {
                result.erase(kvp.key());
            }
            return result;
        });
        descriptor.add("Hash[Any, Any]", "Array[Any]", [](call_context& context) {
            auto result = context.left().require<values::hash>();
            auto& right = context.right().require<values::array>();

            // Remove any elements in left with keys in right
            for (auto const& element : right) {
                result.erase(element);
            }
            return result;
        });
        return descriptor;
    }

}}}}}  // namespace puppet::compiler::evaluation::operators::binary


/*-----------------------------------------------------------------------
Licensed to the Apache Software Foundation (ASF) under one
or more contributor license agceements.  See the NOTICE file
distributed with this work for additional information
regarding copyright ownership.  The ASF licenses this file
to you 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 agceed to in writing,
software distributed under the License is distributed on an
"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
KIND, either express or implied.  See the License for the
specific language governing permissions and limitations
under the License.
-----------------------------------------------------------------------*/
#include "MyOwnDataArrayProtocolHandlers.h"

#include <boost/log/trivial.hpp>

#include <fetpapi/etp/AbstractSession.h>
#include <fetpapi/etp/EtpException.h>
#include <fetpapi/etp/EtpHelpers.h>

#include <fesapi/eml2/AbstractHdfProxy.h>

#include "MyDataObjectRepository.h"

void MyOwnDataArrayProtocolHandlers::on_GetDataArrays(const Energistics::Etp::v12::Protocol::DataArray::GetDataArrays & gda, int64_t correlationId)
{
	Energistics::Etp::v12::Protocol::DataArray::GetDataArraysResponse gdaResponse;

	Energistics::Etp::v12::Protocol::Core::ProtocolException pe;
	for (const std::pair < std::string, Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArrayIdentifier >& element : gda.dataArrays) {
		Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArrayIdentifier dai = element.second;
		BOOST_LOG_TRIVIAL(trace) << "Data array received uri : " << dai.uri;

		try
		{
			COMMON_NS::AbstractObject* obj = repo->getObjectFromUri(dai.uri);
			EML2_NS::AbstractHdfProxy* hdfProxy = dynamic_cast<EML2_NS::AbstractHdfProxy*>(obj);
			if (hdfProxy == nullptr) {
				pe.errors[element.first].message = "The URI points to an object which is not an HDF proxy";
				pe.errors[element.first].code = 9;
				continue;
			}

			BOOST_LOG_TRIVIAL(trace) << "Received pathInResource : " << dai.pathInResource;
			auto elemCountPerDim = hdfProxy->getElementCountPerDimension(dai.pathInResource);
			Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArray da;
			da.dimensions.reserve(elemCountPerDim.size());
			size_t globalElemCount = 1;
			for (size_t i = 0; i < elemCountPerDim.size(); ++i) {
				da.dimensions.push_back(elemCountPerDim[i]);
				globalElemCount *= elemCountPerDim[i];
			}

			auto dt = hdfProxy->getHdfDatatypeInDataset(dai.pathInResource);
			if (dt == COMMON_NS::AbstractObject::DOUBLE)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfDouble avroArray;
				avroArray.values = std::vector<double>(globalElemCount);
				hdfProxy->readArrayNdOfDoubleValues(dai.pathInResource, avroArray.values.data());
				da.data.item.set_ArrayOfDouble(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::FLOAT)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfFloat avroArray;
				avroArray.values = std::vector<float>(globalElemCount);
				hdfProxy->readArrayNdOfFloatValues(dai.pathInResource, avroArray.values.data());
				da.data.item.set_ArrayOfFloat(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::LONG_64 || dt == COMMON_NS::AbstractObject::ULONG_64)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfLong avroArray;
				avroArray.values = std::vector<LONG64>(globalElemCount);
				hdfProxy->readArrayNdOfInt64Values(dai.pathInResource, avroArray.values.data());
				da.data.item.set_ArrayOfLong(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::INT || dt == COMMON_NS::AbstractObject::UINT ||
				dt == COMMON_NS::AbstractObject::SHORT || dt == COMMON_NS::AbstractObject::USHORT)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfInt avroArray;
				avroArray.values = std::vector<int>(globalElemCount);
				hdfProxy->readArrayNdOfIntValues(dai.pathInResource, avroArray.values.data());
				da.data.item.set_ArrayOfInt(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::CHAR || dt == COMMON_NS::AbstractObject::UCHAR)
			{
				std::string avroArray(globalElemCount, '\0');
				hdfProxy->readArrayNdOfCharValues(dai.pathInResource, &avroArray[0]);
				da.data.item.set_bytes(avroArray);
			}

			gdaResponse.dataArrays[element.first] = da;
		}
		catch (ETP_NS::EtpException& ex)
		{
			pe.errors[element.first].message = ex.what();
			pe.errors[element.first].code = ex.getErrorCode();
			continue;
		}
	}

	if (!pe.errors.empty()) {
		session->send(gdaResponse, correlationId);
		session->send(pe, correlationId, 0x02);
	}
	else {
		session->send(gdaResponse, correlationId, 0x02);
	}
}

void MyOwnDataArrayProtocolHandlers::on_PutDataArrays(const Energistics::Etp::v12::Protocol::DataArray::PutDataArrays & pda, int64_t correlationId)
{
	Energistics::Etp::v12::Protocol::Core::ProtocolException pe;
	for (std::pair < std::string, Energistics::Etp::v12::Datatypes::DataArrayTypes::PutDataArraysType > pdat : pda.dataArrays) {
		BOOST_LOG_TRIVIAL(trace) << "PutDataArray in resource " << pdat.second.uid.uri << " at path " << pdat.second.uid.pathInResource;
		for (size_t i = 0; i < pdat.second.array.dimensions.size(); ++i) {
			BOOST_LOG_TRIVIAL(trace) << "Dimension " << i << " with count : " << pdat.second.array.dimensions[i];
		}

		try {
			const std::string uri = pdat.second.uid.uri;
			std::pair<std::string, std::string> uuidAndVersion = ETP_NS::EtpHelpers::getUuidAndVersionFromUri(uri);

			EML2_NS::AbstractHdfProxy* hdfProxy = repo->getDataObjectByUuidAndVersion<EML2_NS::AbstractHdfProxy>(uuidAndVersion.first, uuidAndVersion.second);
			if (hdfProxy == nullptr) {
				BOOST_LOG_TRIVIAL(trace) << "Creating file " << uuidAndVersion.first << ".h5 in " << repo->hdf5Folder;
				hdfProxy = repo->createHdfProxy(uuidAndVersion.first, uuidAndVersion.first, repo->hdf5Folder, uuidAndVersion.first + ".h5", COMMON_NS::DataObjectRepository::openingMode::READ_WRITE);
			}

			size_t lastSlash = pdat.second.uid.pathInResource.rfind('/');
			if (lastSlash == std::string::npos || lastSlash == pdat.second.uid.pathInResource.back()) {
				pe.errors[pdat.first].message = "The pathInResource " + pdat.second.uid.pathInResource + " has got a bad syntax.";
				pe.errors[pdat.first].code = 5;
				continue;
			}

			std::unique_ptr<unsigned long long[]> numValuesInEachDimension(new unsigned long long[pdat.second.array.dimensions.size()]());
			for (size_t i = 0; i < pdat.second.array.dimensions.size(); ++i) {
				numValuesInEachDimension[i] = pdat.second.array.dimensions[i];
			}
			if (pdat.second.array.data.item.idx() == 0) {
				std::unique_ptr<char[]> tmp(new char[pdat.second.array.data.item.get_ArrayOfBoolean().values.size()]());
				for (size_t i = 0; i < pdat.second.array.data.item.get_ArrayOfBoolean().values.size(); ++i) {
					tmp[i] = pdat.second.array.data.item.get_ArrayOfBoolean().values[i] ? 1 : 0;
				}
				hdfProxy->writeArrayNdOfCharValues(pdat.second.uid.pathInResource.substr(0, lastSlash), pdat.second.uid.pathInResource.substr(lastSlash + 1),
					tmp.get(), numValuesInEachDimension.get(), pdat.second.array.dimensions.size());
			}
			else if (pdat.second.array.data.item.idx() == 1) {
				hdfProxy->writeArrayNdOfIntValues(pdat.second.uid.pathInResource.substr(0, lastSlash), pdat.second.uid.pathInResource.substr(lastSlash + 1),
					pdat.second.array.data.item.get_ArrayOfInt().values.data(), numValuesInEachDimension.get(), pdat.second.array.dimensions.size());
			}
			else if (pdat.second.array.data.item.idx() == 2) {
				hdfProxy->writeArrayNdOfInt64Values(pdat.second.uid.pathInResource.substr(0, lastSlash), pdat.second.uid.pathInResource.substr(lastSlash + 1),
					pdat.second.array.data.item.get_ArrayOfLong().values.data(), numValuesInEachDimension.get(), pdat.second.array.dimensions.size());
			}
			else if (pdat.second.array.data.item.idx() == 3) {
				hdfProxy->writeArrayNdOfFloatValues(pdat.second.uid.pathInResource.substr(0, lastSlash), pdat.second.uid.pathInResource.substr(lastSlash + 1),
					pdat.second.array.data.item.get_ArrayOfFloat().values.data(), numValuesInEachDimension.get(), pdat.second.array.dimensions.size());
			}
			else if (pdat.second.array.data.item.idx() == 4) {
				hdfProxy->writeArrayNdOfDoubleValues(pdat.second.uid.pathInResource.substr(0, lastSlash), pdat.second.uid.pathInResource.substr(lastSlash + 1),
					pdat.second.array.data.item.get_ArrayOfDouble().values.data(), numValuesInEachDimension.get(), pdat.second.array.dimensions.size());
			}
			else if (pdat.second.array.data.item.idx() == 5) {
				pe.errors[pdat.first].message = "Putting a string array is not supported yet.";
				pe.errors[pdat.first].code = 7;
				continue;
			}
			else if (pdat.second.array.data.item.idx() == 6) {
				std::unique_ptr<char[]> tmp(new char[pdat.second.array.data.item.get_bytes().size()]());
				for (size_t i = 0; i < pdat.second.array.data.item.get_bytes().size(); ++i) {
					tmp[i] = pdat.second.array.data.item.get_bytes()[i];
				}
				hdfProxy->writeArrayNdOfCharValues(pdat.second.uid.pathInResource.substr(0, lastSlash), pdat.second.uid.pathInResource.substr(lastSlash + 1),
					tmp.get(), numValuesInEachDimension.get(), pdat.second.array.dimensions.size());
			}
			else {
				pe.errors[pdat.first].message = "The index " + std::to_string(pdat.second.array.data.item.idx()) + " of the array is not supported yet.";
				pe.errors[pdat.first].code = 7;
				continue;
			}

			// We need to close the HDF5 file in order to store the writte array
			hdfProxy->close();
		}
		catch (ETP_NS::EtpException& ex)
		{
			pe.errors[pdat.first].message = ex.what();
			pe.errors[pdat.first].code = ex.getErrorCode();
			continue;
		}
	}

	if (!pe.errors.empty()) {
		session->send(pe, correlationId, 0x02);
	}
}

void MyOwnDataArrayProtocolHandlers::on_GetDataSubarrays(const Energistics::Etp::v12::Protocol::DataArray::GetDataSubarrays & msg, int64_t correlationId)
{
	for (auto it = msg.dataSubarrays.begin(); it != msg.dataSubarrays.end(); ++it) {
		Energistics::Etp::v12::Protocol::DataArray::GetDataSubarraysResponse response;
		Energistics::Etp::v12::Protocol::Core::ProtocolException pe;

		auto element = *it;
		Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArrayIdentifier dai = element.second.uid;
		BOOST_LOG_TRIVIAL(trace) << "Data subarray received uri : " << dai.uri;
		for (auto start : element.second.starts) {
			BOOST_LOG_TRIVIAL(trace) << "Data subarray received start : " << start;
		}
		for (auto count : element.second.counts) {
			BOOST_LOG_TRIVIAL(trace) << "Data subarray received count : " << count;
		}

		try
		{
			COMMON_NS::AbstractObject* obj = repo->getObjectFromUri(dai.uri);
			EML2_NS::AbstractHdfProxy* hdfProxy = dynamic_cast<EML2_NS::AbstractHdfProxy*>(obj);
			if (hdfProxy == nullptr) {
				pe.errors[element.first].message = "The URI points to an object which is not an HDF proxy";
				pe.errors[element.first].code = 9;
				continue;
			}

			BOOST_LOG_TRIVIAL(trace) << "Received pathInResource : " << dai.pathInResource;
			Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArray da;
			da.dimensions.reserve(element.second.counts.size());
			size_t globalElemCount = 1;
			for (auto count : element.second.counts) {
				da.dimensions.push_back(count);
				globalElemCount *= count;
			}

			std::unique_ptr<unsigned long long[]> counts(new unsigned long long[element.second.counts.size()]);
			for (size_t i = 0; i < element.second.counts.size(); ++i) {
				counts[i] = element.second.counts[i];
			}
			std::unique_ptr<unsigned long long[]> starts(new unsigned long long[element.second.starts.size()]);
			for (size_t i = 0; i < element.second.starts.size(); ++i) {
				starts[i] = element.second.starts[i];
			}

			auto dt = hdfProxy->getHdfDatatypeInDataset(dai.pathInResource);
			if (dt == COMMON_NS::AbstractObject::DOUBLE)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfDouble avroArray;
				avroArray.values = std::vector<double>(globalElemCount);
				hdfProxy->readArrayNdOfDoubleValues(dai.pathInResource, avroArray.values.data(), counts.get(), starts.get(), element.second.counts.size());
				da.data.item.set_ArrayOfDouble(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::FLOAT)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfFloat avroArray;
				avroArray.values = std::vector<float>(globalElemCount);
				hdfProxy->readArrayNdOfFloatValues(dai.pathInResource, avroArray.values.data(), counts.get(), starts.get(), element.second.counts.size());
				da.data.item.set_ArrayOfFloat(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::LONG_64 || dt == COMMON_NS::AbstractObject::ULONG_64)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfLong avroArray;
				avroArray.values = std::vector<LONG64>(globalElemCount);
				hdfProxy->readArrayNdOfInt64Values(dai.pathInResource, avroArray.values.data(), counts.get(), starts.get(), element.second.counts.size());
				da.data.item.set_ArrayOfLong(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::INT || dt == COMMON_NS::AbstractObject::UINT ||
				dt == COMMON_NS::AbstractObject::SHORT || dt == COMMON_NS::AbstractObject::USHORT)
			{
				Energistics::Etp::v12::Datatypes::ArrayOfInt avroArray;
				avroArray.values = std::vector<int>(globalElemCount);
				hdfProxy->readArrayNdOfIntValues(dai.pathInResource, avroArray.values.data(), counts.get(), starts.get(), element.second.counts.size());
				da.data.item.set_ArrayOfInt(avroArray);
			}
			else if (dt == COMMON_NS::AbstractObject::CHAR || dt == COMMON_NS::AbstractObject::UCHAR)
			{
				pe.errors[element.first].message = "This server does not support slicing on 8 bits values array";
				pe.errors[element.first].code = 7;
				continue;
			}

			response.dataSubarrays[element.first] = da;

			session->send(response, correlationId, std::next(it) == msg.dataSubarrays.end() ? 0x02 : 0);
		}
		catch (ETP_NS::EtpException& ex)
		{
			pe.errors[element.first].message = ex.what();
			pe.errors[element.first].code = ex.getErrorCode();

			session->send(pe, correlationId, std::next(it) == msg.dataSubarrays.end() ? 0x02 : 0);
		}
	}
}

void MyOwnDataArrayProtocolHandlers::on_GetDataArrayMetadata(const Energistics::Etp::v12::Protocol::DataArray::GetDataArrayMetadata & gdam, int64_t correlationId)
{
	Energistics::Etp::v12::Protocol::DataArray::GetDataArrayMetadataResponse gdamResponse;

	Energistics::Etp::v12::Protocol::Core::ProtocolException pe;
	for (std::pair < std::string, Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArrayIdentifier > element : gdam.dataArrays) {
		Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArrayIdentifier& dai = element.second;
		BOOST_LOG_TRIVIAL(trace) << "GetDataArrayMetadata received uri : " << dai.uri;

		try {
			COMMON_NS::AbstractObject* obj = repo->getObjectFromUri(dai.uri);
			EML2_NS::AbstractHdfProxy* hdfProxy = dynamic_cast<EML2_NS::AbstractHdfProxy*>(obj);
			if (hdfProxy == nullptr) {
				pe.errors[element.first].message = "The URI points to an object which is not an HDF proxy";
				pe.errors[element.first].code = 9;
				continue;
			}

			BOOST_LOG_TRIVIAL(trace) << "Received pathInResource : " << dai.pathInResource;
			Energistics::Etp::v12::Datatypes::DataArrayTypes::DataArrayMetadata dam;
			auto elemCountPerDim = hdfProxy->getElementCountPerDimension(dai.pathInResource);
			dam.dimensions.reserve(elemCountPerDim.size());
			unsigned long long globalElemCount = 1;
			for (auto i = 0; i < elemCountPerDim.size(); ++i) {
				dam.dimensions.push_back(elemCountPerDim[i]);
				globalElemCount *= elemCountPerDim[i];
			}

			auto dt = hdfProxy->getHdfDatatypeInDataset(dai.pathInResource);
			if (dt == COMMON_NS::AbstractObject::DOUBLE)
			{
				dam.transportArrayType = Energistics::Etp::v12::Datatypes::AnyArrayType::arrayOfDouble;
				dam.logicalArrayType = Energistics::Etp::v12::Datatypes::AnyLogicalArrayType::arrayOfDouble64LE;
			}
			else if (dt == COMMON_NS::AbstractObject::FLOAT)
			{
				dam.transportArrayType = Energistics::Etp::v12::Datatypes::AnyArrayType::arrayOfFloat;
				dam.logicalArrayType = Energistics::Etp::v12::Datatypes::AnyLogicalArrayType::arrayOfFloat32LE;
			}
			else if (dt == COMMON_NS::AbstractObject::LONG_64 || dt == COMMON_NS::AbstractObject::ULONG_64)
			{
				dam.transportArrayType = Energistics::Etp::v12::Datatypes::AnyArrayType::arrayOfLong;
				dam.logicalArrayType = Energistics::Etp::v12::Datatypes::AnyLogicalArrayType::arrayOfInt64LE;
			}
			else if (dt == COMMON_NS::AbstractObject::INT || dt == COMMON_NS::AbstractObject::UINT ||
				dt == COMMON_NS::AbstractObject::SHORT || dt == COMMON_NS::AbstractObject::USHORT)
			{
				dam.transportArrayType = Energistics::Etp::v12::Datatypes::AnyArrayType::arrayOfInt;
				dam.logicalArrayType = Energistics::Etp::v12::Datatypes::AnyLogicalArrayType::arrayOfInt32LE;
			}
			else if (dt == COMMON_NS::AbstractObject::CHAR || dt == COMMON_NS::AbstractObject::UCHAR)
			{
				dam.transportArrayType = Energistics::Etp::v12::Datatypes::AnyArrayType::bytes;
				dam.logicalArrayType = Energistics::Etp::v12::Datatypes::AnyLogicalArrayType::arrayOfInt8;
			}
			gdamResponse.arrayMetadata[element.first] = dam;
		}
		catch (ETP_NS::EtpException& ex)
		{
			pe.errors[element.first].message = ex.what();
			pe.errors[element.first].code = ex.getErrorCode();
			continue;
		}
	}

	if (!pe.errors.empty()) {
		session->send(gdamResponse, correlationId);
		session->send(pe, correlationId, 0x02);
	}
	else {
		session->send(gdamResponse, correlationId, 0x02);
	}
}


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

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_PROJECTION_HPP
#define BOOST_GEOMETRY_PROJECTIONS_PROJECTION_HPP

#include <string>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>

namespace boost { namespace geometry { namespace projections
{

/*!
    \brief projection virtual base class
    \details class containing virtual methods
    \ingroup projection
    \tparam LL latlong point type
    \tparam XY xy point type
*/

template <typename LL, typename XY>
class projection
{
    protected :
        // see comment above
        //typedef typename geometry::coordinate_type<LL>::type LL_T;
        //typedef typename geometry::coordinate_type<XY>::type XY_T;
        typedef double LL_T;
        typedef double XY_T;

    public :

        typedef LL geographic_point_type; ///< latlong point type
        typedef XY cartesian_point_type;  ///< xy point type

        /// Forward projection, from Latitude-Longitude to Cartesian
        virtual bool forward(LL const& lp, XY& xy) const = 0;

        /// Inverse projection, from Cartesian to Latitude-Longitude
        virtual bool inverse(XY const& xy, LL& lp) const = 0;

        /// Forward projection using lon / lat and x / y separately
        virtual void fwd(LL_T& lp_lon, LL_T& lp_lat, XY_T& xy_x, XY_T& xy_y) const = 0;

        /// Inverse projection using x / y and lon / lat
        virtual void inv(XY_T& xy_x, XY_T& xy_y, LL_T& lp_lon, LL_T& lp_lat) const = 0;

        /// Returns name of projection
        virtual std::string name() const = 0;

        /// Returns parameters of projection
        virtual parameters const& params() const = 0;

        /// Returns mutable parameters of projection
        virtual parameters& mutable_params() = 0;

        virtual ~projection() {}

};

}}} // namespace boost::geometry::projections

#endif


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

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <numeric>
#include <boost/timer/timer.hpp>

#include <litesimd/compare.h>
#include <litesimd/shuffle.h>
#include <litesimd/helpers/containers.h>

bool g_verbose = true;
namespace ls = litesimd;

template< class Cont_T, typename TAG_T >
class bubble
{
public:
	using container_type = Cont_T;
    using value_type     = typename container_type::value_type;
    using simd_type      = ls::simd_type< value_type, TAG_T >;

    void sort( container_type& cont )
    {
        bool sorted;
        size_t end = std::max<int64_t>( 0, static_cast<int64_t>(cont.size()) - array_size );
        do
        {
            sorted = true;
            for( size_t i = 0; i < end; ++i )
            {
                auto cmp = reinterpret_cast<simd_type*>( &cont[i+1] );
                int off = ls::greater_last_index< value_type, TAG_T >( cont[i], *cmp );

                if( off >= 0 )
                {
                    std::swap( cont[i], cont[i + off + 1] );
                    sorted = false;
                }
            }

            for( size_t i = end+1; i < cont.size(); ++i )
            {
                if( cont[i-1] > cont[i] )
                {
                    std::swap( cont[i-1], cont[i] );
                    sorted = false;
                }
            }

        } while( !sorted );
    }

private:
    constexpr static size_t array_size = ls::simd_type< value_type, TAG_T >::simd_size;
};

template< class Cont_T, typename TAG_T >
class bubble2
{
public:
	using container_type = Cont_T;
    using value_type     = typename container_type::value_type;
    using simd_type      = ls::simd_type< value_type, TAG_T >;

    void sort( container_type& cont )
    {
        bool sorted;
        size_t end = std::max<int64_t>( 0, static_cast<int64_t>(cont.size()) - array_size );
        do
        {
            sorted = true;
            auto cmp = *reinterpret_cast<simd_type*>( &cont[0] );
            for( size_t i = 0; i < end; ++i )
            {
                cmp = ls::high_insert< value_type, TAG_T >( cmp, cont[ i + array_size ] );
                int off = ls::greater_last_index< value_type, TAG_T >( cont[i], cmp );

                if( off >= 0 )
                {
                    std::swap( cont[i], cont[i + off + 1] );
                    sorted = false;
                }
            }

            for( size_t i = end; i < cont.size(); ++i )
            {
                if( cont[i-1] > cont[i] )
                {
                    std::swap( cont[i-1], cont[i] );
                    sorted = false;
                }
            }

        } while( !sorted );
    }

private:
    constexpr static size_t array_size = ls::simd_type< value_type, TAG_T >::simd_size;
};

template< class Cont_T, typename TAG_T >
struct stl_sort
{
	using container_type = Cont_T;

    void sort( container_type& cont )
    {
        std::sort( std::begin( cont ), std::end( cont ) );
    }
};

template< class Cont_T, typename TAG_T >
struct basic_bubble_sort
{
	using container_type = Cont_T;
    using iterator = typename container_type::iterator;

    void sort( container_type& cont )
    {
        bool sorted;
        do
        {
            sorted = true;
            auto it = std::begin( cont );
            auto last = it;

            for( ++it; it != std::end( cont ); ++it )
            {
                if( *last > *it )
                {
                    std::swap( *last, *it );
                    sorted = false;
                }
                last = it;
            }

        } while( !sorted );
    }
};

template< class Cont_T, template< typename...> class Sort_T, typename TAG_T >
uint64_t bench( const std::string& name, size_t size, size_t loop )
{
	using container_type = Cont_T;
    using sort_type = Sort_T< container_type, TAG_T >;

    boost::timer::cpu_timer timer;
    container_type org;

    srand(1);
    std::generate_n( std::back_inserter(org), size, &rand );

    sort_type sort;
    container_type warmup( org );
    sort.sort( warmup );

    timer.start();
    for( size_t j = 0; j < loop; ++j )
    {
        container_type temp( org );
        sort.sort( temp );

        auto it = std::begin( temp );
        auto last = it;
        for( ++it; it < std::end( temp ); ++it )
        {
            if( *it < *last )
            {
                std::cout << *last << " gt " << *it << " - ";
            }
            last = it;
        }
    }
    timer.stop();
    if( g_verbose )
        std::cout << "Sort all " << name << ": " << timer.format();

    return timer.elapsed().wall;
}

int main(int argc, char* /*argv*/[])
{
    constexpr size_t runSize = 0x00010000;
    constexpr size_t loop = 1;
    if( argc > 1 )
    {
        g_verbose = false;
        std::cout << "Bubble sort,SSE Bubble 1,SSE Bubble 2,AVX Bubble 1,AVX Bubble 2" << std::endl << std::endl << std::endl << std::endl;
    }
    else
    {
        std::cout << "\nsize: 0x" << std::hex << std::setw(8) << std::setfill( '0') << runSize << std::endl << std::endl;
    }
    while( 1 )
    {
        uint64_t bsort = bench< ls::vector< int32_t >, basic_bubble_sort,
                              ls::sse_tag >( "Bubble sort .....", runSize, loop );
        uint64_t ssesort = bench< ls::vector< int32_t >, bubble,
                                ls::sse_tag >( "SSE Bubble sort .", runSize, loop );
        uint64_t ssesort2 = bench< ls::vector< int32_t >, bubble2,
                                 ls::sse_tag >( "SSE Bubble2 sort ", runSize, loop );
#ifdef LITESIMD_HAS_AVX
        uint64_t avxsort = bench< ls::vector< int32_t >, bubble,
                                ls::avx_tag >( "AVX Bubble sort .", runSize, loop );
        uint64_t avxsort2 = bench< ls::vector< int32_t >, bubble2,
                                 ls::avx_tag >( "AVX Bubble2 sort ", runSize, loop );
#endif

        if( g_verbose )
        {
            bench< ls::vector< int32_t >, stl_sort, void >( "STL sort ........", runSize, loop );

            std::cout
                << std::endl << "SSE Speed up ......: " << std::fixed << std::setprecision(2)
                << static_cast<float>(bsort)/static_cast<float>(ssesort) << "x"
                << std::endl << "SSE2 Speed up .....: " << std::fixed << std::setprecision(2)
                << static_cast<float>(bsort)/static_cast<float>(ssesort2) << "x"
                << std::endl << "SSE2/SSE Speed up .: " << std::fixed << std::setprecision(2)
                << static_cast<float>(ssesort)/static_cast<float>(ssesort2) << "x"

#ifdef LITESIMD_HAS_AVX
                << std::endl << "AVX Speed up ......: " << std::fixed << std::setprecision(2)
                << static_cast<float>(bsort)/static_cast<float>(avxsort) << "x"
                << std::endl << "AVX2 Speed up .....: " << std::fixed << std::setprecision(2)
                << static_cast<float>(bsort)/static_cast<float>(avxsort2) << "x"
                << std::endl << "AVX2/AVX Speed up .: " << std::fixed << std::setprecision(2)
                << static_cast<float>(avxsort)/static_cast<float>(avxsort2) << "x"
#endif

                << std::endl << std::endl;
        }
        else
        {
            std::cout
                << bsort
                << "," << ssesort
                << "," << ssesort2
#ifdef LITESIMD_HAS_AVX
                << "," << avxsort
                << "," << avxsort2
#endif
                << std::endl;
        }
    }
    return 0;
}


#include <string>
#include <vector>

#include <boost/filesystem.hpp>
#include <photon_propagator/device_structs/ice_color.h>

class IceModel{
public:
  IceModel(const boost::filesystem::path& ice_model_path, float scattering_angle);

  void to_device();
  ice_color* __device_ptr; // array of ice_color objects

  size_t n_layers(){ return n_layers_; }
  float layers_spacing(){ return layer_spacing_; }
  float hmin(){ return hmin_; }
  float c_vacuum(){ return c_; }

  void pprint() const;

  size_t size_of() const { return color_slices_.size()*sizeof(ice_color); }

private:

  boost::filesystem::path ice_model_parameters_filename_;
  boost::filesystem::path ice_model_filename_;
  boost::filesystem::path wavelength_data_filename_;
  size_t n_layers_;
  float layer_spacing_;
  float hmin_;
  float c_;
  float g_; // scattering angle
  std::vector<ice_color> color_slices_;

  void configure(const boost::filesystem::path& ice_model_parameters_filename,
		 const boost::filesystem::path& ice_model_filename,
		 const boost::filesystem::path& wavelength_data_filename);

};


// Copyright (c) 2018-2019, CNRS
// Authors: Sanghyun Kim <ggory15@snu.ac.kr>

#include <iostream>

#include <boost/test/unit_test.hpp>

#include <iomanip>
#include <yaml-cpp/yaml.h>
#include <timeopt/interface/contactplanner.hpp>
#include <momentumopt/dynopt/DynamicsOptimizer.hpp>
#include <momentumopt/cntopt/ContactPlanFromFile.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
BOOST_AUTO_TEST_CASE ( test_Time_opt )
{
    std::cout << "test Time Optimization" << std::endl;
    using namespace Eigen;
    using namespace momentumopt;

    std::cout << "Get Contact Information" << std::endl;
    using namespace timeopt;
    using namespace Eigen;

    InitialState init_state;

    init_state.setCOM(Eigen::Vector3d(0.0, 0.15, -0.2));
    init_state.setInitialPose(true, Vector3d(0.086, 0.15,-0.92), Quaterniond(1, 0, 0, 0), RF);
    init_state.setInitialPose(true, Vector3d(-0.086, 0.15,-0.92), Quaterniond(1, 0, 0, 0), LF);
    init_state.setInitialPose(false, Vector3d(0.4, 0.3, 0.0), Quaterniond(1, 0, 0, 0), RH);
    init_state.setInitialPose(false, Vector3d(-0.4, 0.3, 0.0), Quaterniond(1, 0, 0, 0), LH);
    init_state.save();

    timeopt::ContactState contact_state(6);
    contact_state.SetPhase(0, PhaseInfo(RF, 0.0, 1.0, Vector3d(0.086, 0.15,-0.92), Matrix3d::Identity()));
    contact_state.SetPhase(1, PhaseInfo(RF, 2.0, 4.5, Vector3d(0.500, 0.45, -0.76), Matrix3d::Identity()));
    contact_state.SetPhase(2, PhaseInfo(RF, 6.0, 9.9, Vector3d(0.450, 0.98, -0.27), Matrix3d::Identity()));

    contact_state.SetPhase(3, PhaseInfo(LF, 0.0, 2.5, Vector3d(-0.086, 0.15, -0.92), Matrix3d::Identity()));
    contact_state.SetPhase(4, PhaseInfo(LF, 4.0, 6.5, Vector3d(-0.080, 0.70, -0.52), Matrix3d::Identity()));
    contact_state.SetPhase(5, PhaseInfo(LF, 8.5, 9.9, Vector3d(-0.080, 1.25, -0.25), Matrix3d::Identity()));
    contact_state.save();

    ContactPlanner planner;
    std::string planner_path;
    planner_path = std::string(CONFIG_DIR)+"/"+"cfg_momSc_demo01.yaml";

    planner.initialize(planner_path, init_state, contact_state);
    planner.setTimehorizon(contact_state.getTimeHorizon());
    planner.setRobotmass(60.0);
    planner.setFinalCOM(Eigen::Vector3d(0.2, 1.2, 0.4));

    planner.saveToFile();
    std::cout << "Done" << std::endl;
    // Until this, finished planner setting

    std::cout << "Do Time Optimization" << std::endl;
    std::string cfg_file;
    cfg_file = planner_path.substr(0, planner_path.size()-5)+"_final.yaml";
    PlannerSetting planner_setting;
    planner_setting.initialize(cfg_file);

    // define robot initial state
    DynamicsState ini_state;
    ini_state.fillInitialRobotState(cfg_file);

    // define reference dynamic sequence
    momentumopt::DynamicsSequence ref_sequence;
    ref_sequence.resize(planner_setting.get(PlannerIntParam_NumTimesteps));

    // define contact plan
    ContactPlanFromFile contact_plan;
    contact_plan.initialize(planner_setting);
    contact_plan.optimize(ini_state);

    // optimize motion
    DynamicsOptimizer dyn_optimizer;
    dyn_optimizer.initialize(planner_setting, ini_state, &contact_plan);
    dyn_optimizer.optimize(ref_sequence);

    std::cout << "Done" << std::endl;

}
BOOST_AUTO_TEST_SUITE_END ()