#include "lm/interpolate/normalize.hh"

#include "lm/interpolate/interpolate_info.hh"
#include "lm/interpolate/merge_probabilities.hh"
#include "lm/common/ngram_stream.hh"
#include "util/stream/chain.hh"
#include "util/stream/multi_stream.hh"

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

namespace lm { namespace interpolate { namespace {

// log without backoff
const float kInputs[] = {-0.3, 1.2, -9.8, 4.0, -7.0, 0.0};

class WriteInput {
  public:
    WriteInput() {}
    void Run(const util::stream::ChainPosition &to) {
      util::stream::Stream out(to);
      for (WordIndex i = 0; i < sizeof(kInputs) / sizeof(float); ++i, ++out) {
        memcpy(out.Get(), &i, sizeof(WordIndex));
        memcpy((uint8_t*)out.Get() + sizeof(WordIndex), &kInputs[i], sizeof(float));
      }
      out.Poison();
    }
};

void CheckOutput(const util::stream::ChainPosition &from) {
  NGramStream<float> in(from);
  float sum = 0.0;
  for (WordIndex i = 0; i < sizeof(kInputs) / sizeof(float) - 1 /* <s> at the end */; ++i) {
    sum += pow(10.0, kInputs[i]);
  }
  sum = log10(sum);
  BOOST_REQUIRE(in);
  BOOST_CHECK_CLOSE(kInputs[0] - sum, in->Value(), 0.0001);
  BOOST_REQUIRE(++in);
  BOOST_CHECK_CLOSE(kInputs[1] - sum, in->Value(), 0.0001);
  BOOST_REQUIRE(++in);
  BOOST_CHECK_CLOSE(kInputs[2] - sum, in->Value(), 0.0001);
  BOOST_REQUIRE(++in);
  BOOST_CHECK_CLOSE(kInputs[3] - sum, in->Value(), 0.0001);
  BOOST_REQUIRE(++in);
  BOOST_CHECK_CLOSE(kInputs[4] - sum, in->Value(), 0.0001);
  BOOST_REQUIRE(++in);
  BOOST_CHECK_CLOSE(kInputs[5] - sum, in->Value(), 0.0001);
  BOOST_CHECK(!++in);
}

BOOST_AUTO_TEST_CASE(Unigrams) {
  InterpolateInfo info;
  info.lambdas.push_back(2.0);
  info.lambdas.push_back(-0.1);
  info.orders.push_back(1);
  info.orders.push_back(1);

  BOOST_CHECK_EQUAL(0, MakeEncoder(info, 1).EncodedLength());

  // No backoffs.
  util::stream::Chains blank(0);
  util::FixedArray<util::stream::ChainPositions> models_by_order(2);
  models_by_order.push_back(blank);
  models_by_order.push_back(blank);

  util::stream::Chains merged_probabilities(1);
  util::stream::Chains probabilities_out(1);
  util::stream::Chains backoffs_out(0);

  merged_probabilities.push_back(util::stream::ChainConfig(sizeof(WordIndex) + sizeof(float) + sizeof(float), 2, 24));
  probabilities_out.push_back(util::stream::ChainConfig(sizeof(WordIndex) + sizeof(float), 2, 100));

  merged_probabilities[0] >> WriteInput();
  Normalize(info, models_by_order, merged_probabilities, probabilities_out, backoffs_out);

  util::stream::ChainPosition checker(probabilities_out[0].Add());

  merged_probabilities >> util::stream::kRecycle;
  probabilities_out >> util::stream::kRecycle;

  CheckOutput(checker);
  probabilities_out.Wait();
}

}}} // namespaces


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/copy_const_reference.hpp>

#include <scitbx/math/resample.h>

namespace scitbx { namespace math {

namespace {

  struct non_parametric_bootstrap_wrappers
  {
    typedef resample::non_parametric_bootstrap<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("non_parametric_bootstrap", no_init)
        .def(init< scitbx::af::const_ref<double> const&,
                  long const&>
             ((arg("observations"),
               arg("seed") )))
        .def("draw", &w_t::draw)
        .def("draw_from_random_jack_knifed_sample",
             &w_t::draw_from_random_jack_knifed_sample )
        ;
    }

  };

  //----------------------------------------------------------

  struct non_parametric_bootstrap_as_int_wrappers
  {
    typedef resample::non_parametric_bootstrap_as_int<std::size_t> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("non_parametric_bootstrap_as_int", no_init)
        .def(init< scitbx::af::const_ref<std::size_t> const&,
                  long const&>
             ((arg("observations"),
               arg("seed") )))
        .def("draw", &w_t::draw)
        .def("draw_from_random_jack_knifed_sample",
             &w_t::draw_from_random_jack_knifed_sample )
        ;
    }

  };

  //----------------------------------------------------------

  struct smooth_bootstrap_wrappers
  {
    typedef resample::smooth_bootstrap<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<w_t>("smooth_bootstrap", no_init)
        .def(init< scitbx::af::const_ref<double> const&,
                  long const&>
             ((arg("observations"),
               arg("seed") )))
        .def("draw", &w_t::draw)
        .def("draw_from_random_jack_knifed_sample",
             &w_t::draw_from_random_jack_knifed_sample )
        ;
    }

  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_resample()
  {
    non_parametric_bootstrap_wrappers::wrap();
    non_parametric_bootstrap_as_int_wrappers::wrap();
    smooth_bootstrap_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


// g++ example.cpp -std=c++17 -I /path/to/boost/>=1.54/ -lOpenCL

#include <boost/compute.hpp>

#include "large_source_compute.hpp"

BOOST_COMPUTE_FUNCTION(int, awesome_function, (double a, double b), {
    return static_cast<int>(a / b);
});

int main(int, char**) {
    boost::compute::device device = boost::compute::system::default_device();
    boost::compute::context context(device);
    boost::compute::command_queue queue(context, device);

    sep::compute::source::file<double> input1("very_very_large_input1.bin");
    sep::compute::source::file<double> input2("very_very_large_input2.bin");
    sep::compute::sink::file<int> output("output.bin");

    assert(input1.is_valid_type());
    assert(input2.is_valid_type());
    assert(input1.total() == input2.total());

    // memory limit
    const int memory_size = 1024 * 1024 * 1024;
    sep::compute::linear::manager manager(context, queue, memory_size);

    sep::compute::utility::measure_callback cb;
    manager.compute(input1, input2, output, awesome_function, &cb);

    cb.print_measure_report();
    // total: 182980msec; compute: 260.239msec transport: 182720msec.

    return 0;
}


#include "ros/ros.h"
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <hdmap_msgs/gpfpd.h>
#include <nav_msgs/Odometry.h>
#include <std_srvs/Empty.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

#include "common/math_utils.h"
#include "common/transform_utils.h"
#include "fusion/transPointCLoud.h"
#include "fusion/utmProjection.h"
#include <exception>
#include <iostream>
#include <signal.h>
#include <sstream>
#include <stdlib.h>
#include <vector>
//#include "loadExtrinsic.hpp"

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

#include "loadExtrinsic.hpp"

using namespace lidar_slam;

class FPDReceiver {
public:
  struct GPFPD {
    int nGPSWeek;
    double dGPSTime;
    double dHeading;
    double dPitch;
    double dRoll;
    double dLattitude;
    double dLongitude;
    double dAltitude;
    double dVEast;
    double dVNorth;
    double dVUp;
    double dBaseline;
    int nSatelitesNum1;
    int nSatelitesNum2;
    char cStatus[2];
    char cReserved[3];
  };

  FPDReceiver(ros::NodeHandle &node, std::string mode_ = "",
              std::string extrinsic_file_ = "") {
    mode = mode_;
    subGPSfpd = node.subscribe<hdmap_msgs::gpfpd>(
        "/sensor/gpfpd", 2, &FPDReceiver::gpfpdHandler, this);
    pubOml = node.advertise<nav_msgs::Odometry>("/fpd", 1);
    Oml.header.frame_id = "/map";
    Oml.child_frame_id = "/velodyne";
    TFml.frame_id_ = "/map";
    TFml.child_frame_id_ = "/velodyne";

    // just for lidar mapping
    pubOml_initMap = node.advertise<nav_msgs::Odometry>("/init_lidar2map", 1);
    Oml_initMap.header.frame_id = "/map";
    Oml_initMap.child_frame_id = "/lidar_init";
    TFml_initMap.frame_id_ = "/map";
    TFml_initMap.child_frame_id_ = "/lidar_init";

    // just for init lidar relocalization module
    pubOml_initLoc = node.advertise<geometry_msgs::PoseWithCovarianceStamped>(
        "/initialpose2", 1);

    _initLocSrv = node.advertiseService("/initLoc", &FPDReceiver::initLoc, this);

    ros::NodeHandle privateNode("~");
    std::string extrinsic_file;

    if (extrinsic_file_ == "") {
      if (privateNode.getParam("extrinsic_file", extrinsic_file)) {
        ROS_INFO_STREAM("Set extrinsic file:" << extrinsic_file);
      } else {
        ROS_ERROR("NO extrinsic file setup.");
      }
    } else {
      extrinsic_file = extrinsic_file_;
    }

    if (loadExtrinsic(extrinsic_file, Tli)) {
      ROS_INFO_STREAM("imu2lidar calibration Tli:\n"
                      << Tli.matrix() << std::endl);
    } else {
      ROS_ERROR_STREAM("Error in " << extrinsic_file);
    }

    privateNode.param("latitude", _latitude, 39.86966052);
    privateNode.param("longitude", _longitude, 116.17656364);
    privateNode.param("altitude", _altitude, 62.41314815);
    ROS_INFO("Map origin, latitude:%.8f, longitude:%.8f, altitude:%.4f",_latitude,_longitude,_altitude);
    // map origin in world frame
    wgs2utm_proj4(_longitude, _latitude, _altitude, Vwm_offset[0],
                  Vwm_offset[1], Vwm_offset[2]);
    ROS_INFO_STREAM("Map origin utm:\n" << std::fixed << Vwm_offset);
    initialize = false;
  }

  bool initLoc(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) {
    geometry_msgs::PoseWithCovarianceStamped Oml_initLoc;
    Oml_initLoc.header.stamp = Oml.header.stamp;
    Oml_initLoc.header.frame_id = "map";
    Oml_initLoc.pose.pose.orientation.x = Oml.pose.pose.orientation.x;
    Oml_initLoc.pose.pose.orientation.y = Oml.pose.pose.orientation.y;
    Oml_initLoc.pose.pose.orientation.z = Oml.pose.pose.orientation.z;
    Oml_initLoc.pose.pose.orientation.w = Oml.pose.pose.orientation.w;
    Oml_initLoc.pose.pose.position.x = Oml.pose.pose.position.x;
    Oml_initLoc.pose.pose.position.y = Oml.pose.pose.position.y;
    Oml_initLoc.pose.pose.position.z = Oml.pose.pose.position.z;
    pubOml_initLoc.publish(Oml_initLoc);
    return true;
  }

  void gpfpdHandler(hdmap_msgs::gpfpd fpd) {

    Eigen::Vector3d utm_pos;
    wgs2utm_proj4(fpd.dLongitude, fpd.dLattitude, fpd.dAltitude, utm_pos[0],
                  utm_pos[1], utm_pos[2]);

    // std::cout << "utm_pos:" << utm_pos << std::endl;
    std::vector<double> pos_data;
    pos_data.push_back(utm_pos[1]);
    pos_data.push_back(utm_pos[0]);
    pos_data.push_back(utm_pos[2]);
    pos_data.push_back(fpd.dRoll);
    pos_data.push_back(fpd.dPitch);
    pos_data.push_back(fpd.dHeading);

    Eigen::Matrix4Xd Twi;
    Eigen::Matrix4Xd Twi_inv;
    transMatrixContruct(pos_data, Twi, Twi_inv);
    // std::cout << std::fixed << "Twi:" << Twi << std::endl;
    Eigen::Matrix4Xd Tmi = Twi;
    Tmi.block(0, 3, 3, 1) -= Vwm_offset;
    // std::cout << std::fixed << "Tmi:" << Tmi << std::endl;

    Eigen::Matrix4Xd Tml = Tmi * Tli.inverse().matrix();
    // std::cout << std::fixed << "Tml:" << Tml << std::endl;

    // just for init lidar relocalization module
    if (!initialize && mode == "loc") {
      geometry_msgs::PoseWithCovarianceStamped Oml_initLoc;

      Eigen::Matrix4Xd l_odom = Tml.matrix();
      Eigen::Matrix3d Rml = l_odom.block(0, 0, 3, 3);
      Eigen::Vector3d Vml = l_odom.block(0, 3, 3, 1);
      Eigen::Quaterniond Qml(Rml);

      Oml_initLoc.header.stamp = fpd.header.stamp;
      Oml_initLoc.header.frame_id = "map";
      Oml_initLoc.pose.pose.orientation.x = Qml.x();
      Oml_initLoc.pose.pose.orientation.y = Qml.y();
      Oml_initLoc.pose.pose.orientation.z = Qml.z();
      Oml_initLoc.pose.pose.orientation.w = Qml.w();
      Oml_initLoc.pose.pose.position.x = Vml(0);
      Oml_initLoc.pose.pose.position.y = Vml(1);
      Oml_initLoc.pose.pose.position.z = Vml(2);
      pubOml_initLoc.publish(Oml_initLoc);
    }

    Eigen::Matrix3d Rml = Tml.block(0, 0, 3, 3);
    Eigen::Vector3d Vml = Tml.block(0, 3, 3, 1);
    // std::cout << std::fixed << "Vml:" << Vml << std::endl;

    Eigen::Quaterniond Qml(Rml);
    // std::cout << std::fixed << "Qml:" << Qml.w() << "    " << Qml.x() << " "
    //          << Qml.y() << "    " << Qml.z() << std::endl;
    if (!initialize) {
      Vml_init = Vml;
      Qml_init = Qml;
      initialize = true;
      // std::cout << "Qml_init:" << Qml_init.coeffs() << std::endl;
    }

    Oml.header.stamp = fpd.header.stamp;
    Oml.pose.pose.orientation.x = Qml.x();
    Oml.pose.pose.orientation.y = Qml.y();
    Oml.pose.pose.orientation.z = Qml.z();
    Oml.pose.pose.orientation.w = Qml.w();
    Oml.pose.pose.position.x = Vml(0);
    Oml.pose.pose.position.y = Vml(1);
    Oml.pose.pose.position.z = Vml(2);
    Oml.twist.twist.linear.x = fpd.dVEast;
    Oml.twist.twist.linear.y = fpd.dVNorth;
    Oml.twist.twist.linear.z = fpd.dVUp;
    // printf("/fpd global position: (x, y, z) = (%.3f %.3f %.3f)\n",
    // std::round(Vml(0) / 50.0), std::round(Vml(1) / 50.0), std::round(Vml(2) /
    // 50.0));
    pubOml.publish(Oml);

    TFml.stamp_ = fpd.header.stamp;
    TFml.setRotation(tf::Quaternion(Qml.x(), Qml.y(), Qml.z(), Qml.w()));
    TFml.setOrigin(tf::Vector3(Vml[0], Vml[1], Vml[2]));
    _tfBroadcaster.sendTransform(TFml);

    if (mode == "map") {
      Oml_initMap.header.stamp = fpd.header.stamp;
      Oml_initMap.pose.pose.orientation.x = Qml_init.x();
      Oml_initMap.pose.pose.orientation.y = Qml_init.y();
      Oml_initMap.pose.pose.orientation.z = Qml_init.z();
      Oml_initMap.pose.pose.orientation.w = Qml_init.w();
      Oml_initMap.pose.pose.position.x = Vml_init(0);
      Oml_initMap.pose.pose.position.y = Vml_init(1);
      Oml_initMap.pose.pose.position.z = Vml_init(2);
      Oml_initMap.twist.twist.linear.x = fpd.dVEast;
      Oml_initMap.twist.twist.linear.y = fpd.dVNorth;
      Oml_initMap.twist.twist.linear.z = fpd.dVUp;
      pubOml_initMap.publish(Oml_initMap);
      TFml_initMap.stamp_ = fpd.header.stamp;
      TFml_initMap.setRotation(tf::Quaternion(Qml_init.x(), Qml_init.y(),
                                              Qml_init.z(), Qml_init.w()));
      TFml_initMap.setOrigin(
          tf::Vector3(Vml_init(0), Vml_init(1), Vml_init(2)));
      _tfBroadcaster.sendTransform(TFml_initMap);
    }
  }

private:
  std::string mode;

  // ros something
  ros::Subscriber subGPSfpd;
  tf::TransformBroadcaster _tfBroadcaster;
  ros::Publisher pubOml;
  nav_msgs::Odometry Oml;
  tf::StampedTransform TFml;

  ros::Publisher pubOml_initMap;
  nav_msgs::Odometry Oml_initMap;
  tf::StampedTransform TFml_initMap;

  ros::Publisher pubOml_initLoc;
  nav_msgs::Odometry Oml_initLoc;
  tf::StampedTransform TFml_init;

  ros::ServiceServer _initLocSrv;

  //
  bool initialize;
  Eigen::Vector3d Vml_init;
  Eigen::Quaterniond Qml_init;

  Eigen::Vector3d Vwm_offset;
  Eigen::Isometry3d Tli;

  double _latitude;
  double _longitude;
  double _altitude;
};

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

  ros::init(argc, argv, "gpfpd_receiver");
  ros::NodeHandle n;

  std::string mode;
  if (argc <= 1) {
    ROS_INFO("Run in default mode, option: map|loc");
  } else if (argc == 2) {
    mode = argv[1];
    ROS_INFO("Run in mode: %s", mode.c_str());
    FPDReceiver fpd_receiver(n, mode);
    ros::spin();
  } else if (argc == 3) {
    mode = argv[1];
    ROS_INFO("Run in mode: %s", mode.c_str());
    std::string extrinsic_file = argv[2];
    FPDReceiver fpd_receiver(n, mode, extrinsic_file);
    ros::spin();
  }

  return 0;
}


/**************************************************************************
** This file is a part of our work (Siggraph'16 paper, binary, code and dataset):
**
** Roto++: Accelerating Professional Rotoscoping using Shape Manifolds
** Wenbin Li, Fabio Viola, Jonathan Starck, Gabriel J. Brostow and Neill D.F. Campbell
**
** w.li AT cs.ucl.ac.uk
** http://visual.cs.ucl.ac.uk/pubs/rotopp
**
** Copyright (c) 2016, Wenbin Li
** 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 and data 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.
**
** THIS WORK AND THE RELATED SOFTWARE, SOURCE CODE AND DATA 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.
***************************************************************************/

#ifndef KERNEL_HPP
#define KERNEL_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/SparseCore>

#include <string>
#include <iostream>
#include <cassert>
#include <memory>

#include "include/rotoSolver/baseDefs.hpp"

class Kernel {
public:
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::VectorXd Vector;
    typedef Eigen::SparseMatrix<double> SparseMatrix;
    typedef Eigen::MatrixXd ExtraDataType;

    virtual Kernel* createCopy() const = 0;

    virtual int NumberOfParameters() const = 0;

    virtual void SetParameters(const Vector& logParams) = 0;

    virtual void GetParameters(Vector& logParams) const = 0;

    virtual void Print() const = 0;

    virtual Matrix GetKernelMatrix(const Matrix& X, const Matrix& Z,
                                   ExtraDataType* extraDataPtr = NULL) const = 0;

    virtual Vector GetDiagonalOfKernel(const Matrix& X) const = 0;

    virtual std::vector< shared_ptr< Matrix > >
    GetGradientWrtParams(const Matrix& X, const Matrix& K,
                         ExtraDataType* extraDataPtr) const = 0;

    virtual shared_ptr< SparseMatrix >
    GetGradientWrtData(const Matrix& X, const Matrix& K,
                       ExtraDataType* extraDataPtr) const = 0;

    virtual shared_ptr< SparseMatrix >
    GetGradientWrtSecondTerm(const Matrix& X, const Matrix& Z, const Matrix& K,
                             ExtraDataType* extraDataPtr = NULL) const = 0;

    virtual void AddParametersToVariableMap(
            std::map<std::string, shared_ptr<Matrix> >& variableMap) const = 0;

};

class RbfKernel : public Kernel
{
private:
    double _alpha;
    double _gamma;
    static const int NUMBER_PARAMETERS = 2;

public:
    typedef Eigen::MatrixXd Matrix;
    typedef Eigen::VectorXd Vector;
    typedef Eigen::SparseMatrix<double> SparseMatrix;
    typedef Eigen::MatrixXd ExtraDataType;

public:
    explicit RbfKernel();

    explicit RbfKernel(const double alpha, const double gamma);

    virtual Kernel* createCopy() const;

    virtual int NumberOfParameters() const;

    virtual void SetParameters(const Vector& logParams);

    virtual void GetParameters(Vector& logParams) const;

    virtual void Print() const;

    virtual Matrix GetKernelMatrix(const Matrix& X, const Matrix& Z,
                                   ExtraDataType* extraDataPtr = NULL) const;

    virtual Vector GetDiagonalOfKernel(const Matrix& X) const;

    virtual std::vector< shared_ptr< Matrix > >
    GetGradientWrtParams(const Matrix& X, const Matrix& K,
                         ExtraDataType* extraDataPtr = NULL) const;

    virtual shared_ptr< SparseMatrix >
    GetGradientWrtData(const Matrix& X, const Matrix& K,
                       ExtraDataType* extraDataPtr = NULL) const;

    virtual shared_ptr< SparseMatrix >
    GetGradientWrtSecondTerm(const Matrix& X, const Matrix& Z, const Matrix& K,
                             ExtraDataType* extraDataPtr = NULL) const;

    virtual void AddParametersToVariableMap(
            std::map<std::string, shared_ptr<Matrix> >& variableMap) const;
};

#endif // KERNEL_HPP


// Copyright 2019, Intel Corporation

#include "pmlc/dialect/stripe/analysis.h"

#include <utility>

#include <utility>

#include <boost/math/common_factor.hpp>

namespace pmlc {
namespace dialect {
namespace stripe {

AffinePolynomial::AffinePolynomial() : constant(0) {}

AffinePolynomial::AffinePolynomial(int64_t x) : constant(x) {}

AffinePolynomial::AffinePolynomial(Value* x) : constant(0) {
  if (auto ba = mlir::dyn_cast<mlir::BlockArgument>(x)) {
    // This is a basic index: done
    terms.emplace(ba, 1);
    return;
  }
  auto dop = x->getDefiningOp();
  if (auto op = mlir::dyn_cast<AffineConstOp>(dop)) {
    // This is a constant
    constant = op.value().getSExtValue();
  } else if (auto op = mlir::dyn_cast<AffineMulOp>(dop)) {
    *this = AffinePolynomial(op.input());
    *this *= op.scale().getSExtValue();
  } else if (auto op = mlir::dyn_cast<AffineAddOp>(dop)) {
    for (Value* v : op.inputs()) {
      *this += AffinePolynomial(v);
    }
  } else if (auto op = mlir::dyn_cast<AffineMeta>(dop)) {
    *this = AffinePolynomial(op.input());
  } else {
    throw std::runtime_error("Invalid affine in ComputeAffineRange");
  }
}

AffinePolynomial& AffinePolynomial::operator*=(int64_t x) {
  constant *= x;
  if (x == 0) {
    terms.clear();
  } else {
    for (auto& kvp : terms) {
      kvp.second *= x;
    }
  }
  return *this;
}

AffinePolynomial& AffinePolynomial::operator+=(const AffinePolynomial& x) {
  constant += x.constant;
  for (const auto& kvp : x.terms) {
    terms[kvp.first] += kvp.second;
    if (terms[kvp.first] == 0) {
      terms.erase(kvp.first);
    }
  }
  return *this;
}

bool AffinePolynomial::operator<(const AffinePolynomial& rhs) const {
  if (constant < rhs.constant) {
    return true;
  }
  if (constant > rhs.constant) {
    return false;
  }
  return terms < rhs.terms;
}

bool AffinePolynomial::operator==(const AffinePolynomial& rhs) const {
  return constant == rhs.constant && terms == rhs.terms;
}

AffineRange::AffineRange(int64_t _min, int64_t _max, uint64_t _stride) : min(_min), max(_max), stride(_stride) {
  if (min == max) {
    stride = 0;
  }
}

AffineRange& AffineRange::operator*=(int64_t x) {
  min *= x;
  max *= x;
  if (x < 0) {
    std::swap(min, max);
  }
  stride *= x;
  return *this;
}

AffineRange& AffineRange::operator+=(const AffineRange& x) {
  min += x.min;
  max += x.max;
  stride = boost::math::gcd(stride, x.stride);
  return *this;
}

AffineRange& AffineRange::operator|=(const AffineRange& x) {
  min = std::min(min, x.min);
  max = std::max(max, x.max);
  stride = boost::math::gcd(stride, x.stride);
  return *this;
}

AffineRange::AffineRange(const AffinePolynomial& poly) : min(poly.constant), max(poly.constant), stride(0) {
  // Go over each term and adjust
  for (const auto& kvp : poly.terms) {
    // Update the stride
    stride = boost::math::gcd(stride, uint64_t(std::abs(kvp.second)));
    // Extract the parallel for this affine is an argument of
    auto pf = mlir::cast<ParallelForOp>(kvp.first->getOwner()->getParentOp());
    // Extract the appropriate attribute from ranges
    Attribute ra = pf.ranges().getValue()[kvp.first->getArgNumber()];
    // Turn the range into an integer
    int64_t range = ra.cast<IntegerAttr>().getInt();
    // Update min/max
    if (kvp.second >= 0) {
      max += (range - 1) * kvp.second;
    } else {
      min += (range - 1) * kvp.second;
    }
  }
}

bool FlatTensorAccess::operator<(const FlatTensorAccess& rhs) const {
  if (base != rhs.base) {
    return base < rhs.base;
  }
  return access < rhs.access;
}

bool FlatTensorAccess::operator==(const FlatTensorAccess& rhs) const {
  return base == rhs.base && access == rhs.access;
}

FlatTensorAccess ComputeAccess(Value* tensor) {
  FlatTensorAccess r;
  auto bop = tensor->getDefiningOp();
  if (auto op = mlir::dyn_cast<AllocateOp>(bop)) {
    r.base = op;
    r.access.resize(op.layout().dims().size());
  } else if (auto op = mlir::dyn_cast<RefineOp>(bop)) {
    r = ComputeAccess(op.in());
    for (size_t i = 0; i < r.access.size(); i++) {
      r.access[i] += AffinePolynomial(*(op.offsets().begin() + i));
    }
  } else {
    throw std::runtime_error("Invalid tensor value in ComputeAccess");
  }
  return r;
}

}  // namespace stripe
}  // namespace dialect
}  // namespace pmlc


#include <ros/ros.h>

#include <eigen_conversions/eigen_msg.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit_msgs/PlanningScene.h>

#include <control_msgs/FollowJointTrajectoryActionFeedback.h>

#include <fstream>

#include <boost/bind.hpp>

static const std::string PLANNING_GROUP = "panda_arm";

void feedbackCallBack(const boost::shared_ptr<control_msgs::FollowJointTrajectoryActionFeedback const> msg,
                      moveit::planning_interface::MoveGroupInterface &group)
{
  // opening csv file to store the data
  std::ofstream output;
  output.open("../../../src/panda_simulation/error.csv", std::ios::app);

  // points from feeedback message from gazebo
  auto &actual = msg->feedback.actual;
  auto &desired = msg->feedback.desired;

  // getting kinematic model for forward kinematics
  auto kinematic_model = group.getRobotModel();
  robot_state::RobotState state(kinematic_model);

  // writing desired pose of the end effector
  state.setJointGroupPositions(kinematic_model->getJointModelGroup("panda_arm"), desired.positions);
  Eigen::Affine3d eff = state.getGlobalLinkTransform("panda_link8");
  output << msg->feedback.header.seq << " " << eff.translation()[0] << " " << eff.translation()[1] << " "
         << eff.translation()[2] << " ";

  // writing actual pose of the end effector
  state.setJointGroupPositions(kinematic_model->getJointModelGroup("panda_arm"), actual.positions);
  eff = state.getGlobalLinkTransform("panda_link8");
  output << eff.translation()[0] << " " << eff.translation()[1] << " " << eff.translation()[2] << " ";

  // writing desired joint positions
  for (const auto &j : desired.positions)
  {
    output << j << " ";
  }

  // writing actual joint positions
  for (const auto &j : actual.positions)
  {
    output << j << " ";
  }

  output << "\n";

  output.close();
}

int main(int argc, char **argv)
{
  ROS_INFO("RUNNING robot_ab_control_node");
  ros::init(argc, argv, "robot_ab_control_node");

  ros::AsyncSpinner spinner(1);
  spinner.start();

  ros::NodeHandle node_handle;

  // creating visual tools instance to be able control test from rviz motion planning tab
  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
  visual_tools.deleteAllMarkers();
  visual_tools.loadRemoteControl();

  // creating move group for panda manipulator
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // creating move group planning scene publisher
  ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene",1);
  ros::WallDuration sleep_t(0.5);
  while(planning_scene_diff_publisher.getNumSubscribers() < 1)
  {
    sleep_t.sleep();
  }
  moveit_msgs::PlanningScene planning_scene;

  // adding collision object
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = "world";
  collision_object.id = "ground";

  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 10.0;
  primitive.dimensions[1] = 10.0;
  primitive.dimensions[2] = 1.0;

  geometry_msgs::Pose floor_pose;
  floor_pose.orientation.w = 1;
  floor_pose.orientation.x = 0;
  floor_pose.orientation.y = 0;
  floor_pose.orientation.z = 0;

  floor_pose.position.x = 0;
  floor_pose.position.y = 0;
  floor_pose.position.z = -0.5*primitive.dimensions[2];

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(floor_pose);
  collision_object.operation = collision_object.ADD;

  collision_object.header.frame_id = move_group.getPlanningFrame();
  planning_scene.world.collision_objects.push_back(collision_object);
  planning_scene.is_diff = true;
  planning_scene_diff_publisher.publish(planning_scene);

  // subscribing to gazebo feedback messages which contain trajectory information
  ros::Subscriber gazebo_feedback = node_handle.subscribe<control_msgs::FollowJointTrajectoryActionFeedback>(
      "/panda_arm_controller/follow_joint_trajectory/feedback", 1,
      boost::bind(feedbackCallBack, _1, boost::ref(move_group)));

  // cleaning previous file with errors
  std::remove("../../../src/panda_simulation/error.csv");

  const robot_state::JointModelGroup *joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // TASK 1
  int N = 5;
  for (int i = 0; i < N; i++)
  {
    visual_tools.prompt("Press next to continue TASK_1\n");
    visual_tools.deleteAllMarkers();

    ROS_INFO("Step number %i/%i", i + 1, N);

    // current pose of end effector
    Eigen::Isometry3d cur_end_effector = move_group.getCurrentState()->getGlobalLinkTransform("panda_link8");
    geometry_msgs::PoseStamped cur_pose;
    tf::poseEigenToMsg(cur_end_effector, cur_pose.pose);
    visual_tools.publishAxisLabeled(cur_pose.pose, "A", rviz_visual_tools::LARGE);

    // RANDOM POINTS OBTAINED FROM TASK SPACE
    geometry_msgs::PoseStamped trg_pose = move_group.getRandomPose("panda_link8");
    ROS_INFO_STREAM("Next point to plan to : \nt = \n"
                    << trg_pose.pose.position << "\nR = \n"
                    << trg_pose.pose.orientation);
    visual_tools.publishAxisLabeled(trg_pose.pose, "B", rviz_visual_tools::LARGE);
    visual_tools.trigger();

    // setting target pose for move_group
    move_group.setPoseTarget(trg_pose, "panda_link8");

    // do the planning
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO("Planning state = %s", success ? "SUCCESS" : "FAILED");

    // trajectory visualization
    visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
    visual_tools.trigger();

    // executing planned trajectory
    move_group.execute(my_plan);
  }

  visual_tools.prompt("Press next to finish TASK_1 and return manipulator home\n");
  ROS_INFO("Returning home...");
  visual_tools.deleteAllMarkers();

  // moving manipulator to home position
  geometry_msgs::Pose home_pose;
  home_pose.position.x = 0.5;
  home_pose.position.y = 0;
  home_pose.position.z = 0.5;
  home_pose.orientation.w = 1.0;
  visual_tools.publishAxisLabeled(home_pose, "HOME", rviz_visual_tools::LARGE);

  ROS_INFO_STREAM("Next point to plan to : \nt = \n" << home_pose.position << "\nR = \n" << home_pose.orientation);

  moveit::core::RobotState home_state(*move_group.getCurrentState());
  home_state.setFromIK(joint_model_group, home_pose, 10, 0.2);

  move_group.setJointValueTarget(home_state);
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO("Planning state = %s", success ? "SUCCESS" : "FAILED");

  // trajectory visualization
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();

  // executing planned trajectory
  move_group.execute(my_plan);
  ROS_INFO("Done");

  ros::waitForShutdown();
  return 0;
}


#include <boost/numeric/odeint/iterator/integrate/integrate_const.hpp>


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

    @Copyright 2016 Numscale SAS

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_AVERAGE_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( average_
                          , (typename A0, typename A1)
                          , bs::avx2_
                          , bs::pack_<bd::uint8_<A0>, bs::avx_>
                          , bs::pack_<bd::uint8_<A1>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A1 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm256_avg_epu8(a0, a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( average_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::uint16_<A0>, bs::avx_>
                          , bs::pack_<bd::uint16_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0 & a0, const A0 & a1 ) const BOOST_NOEXCEPT
    {
      return _mm256_avg_epu16(a0, a1);
    }
  };
} } }

#endif


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-bitwise
    This function object returns the bitwise selection of bits from the second
    or third operand according to the setting of the bits
    of the first operand

    @par Header <boost/simd/function/bitwise_select.hpp>

    @pre  The operands must share the same bit size and number of elements.

    @par Example:

      @snippet bitwise_select.cpp bitwise_select

    @par Possible output:

      @snippet bitwise_select.txt bitwise_select

  **/
  Value1 bitwise_select(Value0 const& sel, Value1 const& x, Value2 const& y);
} }
#endif

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

#endif


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/algorithm/generate.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Assigns values, generated by given function object g, to the first n elements
    in the range beginning at first.

    @param first  Beginning of the range of elements
    @param n      number of the elements to generate
    @param g      unary function object to generate_n elements

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

      - @c f must be a polymorphic unary function object, i.e callable on generic types.
      - If n <= 0 does nothing

    @par Example:

      @snippet generate_n.cpp generate_n

    @par Possible output:

      @snippet generate_n.txt generate_n

      @return Pointer to the element past the last element generated.
  **/
  template<typename T, typename Size, typename Gen>
  const T* generate_n(T * first, Size n, Gen g)
  {
    if (n > Size(0))
    {
      generate(first, first+n, g);
      return first+n;
    }

    return first;
  }
} }

#endif


/* classifier.cc
   Jeremy Barnes, 6 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.

   Implementation of basic classifier methods.
*/

#include "classifier.h"
#include "classifier_persist_impl.h"
#include "jml/arch/threads.h"
#include "jml/utils/file_functions.h"
#include "evaluation.h"
#include "config_impl.h"
#include "jml/utils/worker_task.h"
#include "jml/utils/guard.h"
#include "dense_features.h"
#include "jml/utils/smart_ptr_utils.h"
#include "jml/utils/vector_utils.h"
#include <boost/bind.hpp>
#include <boost/thread/tss.hpp>
#include "jml/utils/exc_assert.h"
#include "jml/math/xdiv.h"
#include "jml/utils/filter_streams.h"

using namespace std;
using namespace DB;

namespace ML {

/*****************************************************************************/
/* OUTPUT_ENCODING                                                           */
/*****************************************************************************/

std::string print(Output_Encoding encoding)
{
    switch (encoding) {
    case OE_PROB:   return "PROB";
    case OE_PM_INF: return "PM_INF";
    case OE_PM_ONE: return "PM_ONE";
    default:        return format("Output_Encoding(%d)", encoding);
    }
}

BYTE_PERSISTENT_ENUM_IMPL(Output_Encoding);

/*****************************************************************************/
/* OPTIMIZATION_INFO                                                         */
/*****************************************************************************/

void
Optimization_Info::
apply(const Feature_Set & fset, float * output) const
{
    if (fset.size() != indexes.size())
        throw Exception("Optimization_Info::apply(): size mismatch");

    for (unsigned i = 0;  i < indexes.size();  ++i)
        if (indexes[i] != -1)
            output[indexes[i]] = fset[i].second;
}

void
Optimization_Info::
apply(const std::vector<float> & fset, float * output) const
{
    if (fset.size() != indexes.size())
        throw Exception("Optimization_Info::apply(): size mismatch");

    for (unsigned i = 0;  i < indexes.size();  ++i)
        if (indexes[i] != -1)
            output[indexes[i]] = fset[i];
}

void
Optimization_Info::
apply(const float * fset, float * output) const
{
    for (unsigned i = 0;  i < indexes.size();  ++i)
        if (indexes[i] != -1)
            output[indexes[i]] = fset[i];
}

int
Optimization_Info::
get_optimized_index(const Feature & feature) const
{
    map<Feature, int>::const_iterator it
        = feature_to_optimized_index.find(feature);
    if (it == feature_to_optimized_index.end())
        throw Exception("didn't find optimized feature index");
    return it->second;
}

/*****************************************************************************/
/* EXPLANATION                                                               */
/*****************************************************************************/

Explanation::
Explanation(std::shared_ptr<const Feature_Space> fspace,
            double weight)
    : bias(0.0), weight(weight), fspace(fspace)
{
    ExcAssert(fspace);
}

void
Explanation::
add(const Explanation & other, double weight)
{
    if (!fspace)
        fspace = other.fspace;

    for (Feature_Weights::const_iterator
             it = other.feature_weights.begin(),
             end = other.feature_weights.end();
         it != end;  ++it)
        feature_weights[it->first] += weight * it->second;

    bias += weight * other.bias;

    this->weight += other.weight;
}

struct Sort_On_Abs_Second {
    template<class X>
    bool operator () (const X & x1, const X & x2)
    {
        return abs(x1.second) > abs(x2.second);
    }
};

std::string
Explanation::
explain(int nfeatures, const Feature_Set & fset, int) const
{
    // Rank the features
    vector<pair<Feature, float> > ranked(feature_weights.begin(),
                                         feature_weights.end());

    std::sort(ranked.begin(), ranked.end(),
              Sort_On_Abs_Second());

    std::string result;
    if (bias != 0.0)
        result += format("%12.6f                      BIAS\n",
                         bias);
    for (unsigned i = 0;  i < ranked.size() && i < nfeatures;  ++i) {
        const Feature & feature = ranked[i].first;
        float score = ranked[i].second;
        result += format("%12.6f %-20s %s\n",
                         score,
                         fspace->print(feature, (fset)[feature]).c_str(),
                         fspace->print(feature).c_str());
    }

    double total = bias;
    for (unsigned i = 0;  i < ranked.size();  ++i)
        total += ranked[i].second;

    result += format("%12.6f                      TOTAL\n",
                     total);

    return result;
}

std::string
Explanation::
explain(int nfeatures) const
{
    // Rank the features
    vector<pair<Feature, float> > ranked(feature_weights.begin(),
                                         feature_weights.end());

    std::sort(ranked.begin(), ranked.end(),
              Sort_On_Abs_Second());

    std::string result;
    if (bias != 0.0)
        result += format("%12.6f BIAS\n",
                         bias);
    for (unsigned i = 0;  i < ranked.size() && i < nfeatures;  ++i) {
        const Feature & feature = ranked[i].first;
        float score = ranked[i].second;
        result += format("%12.6f %s\n",
                         score,
                         fspace->print(feature).c_str());
    }

    double total = bias;
    for (unsigned i = 0;  i < ranked.size();  ++i)
        total += ranked[i].second;

    result += format("%12.6f TOTAL\n",
                     total);

    return result;
}

std::vector<std::pair<Feature, float> >
Explanation::
explainRaw(int nfeatures, bool includeBias) const
{
    // Rank the features
    vector<pair<Feature, float> > ranked(feature_weights.begin(),
                                         feature_weights.end());

    std::sort(ranked.begin(), ranked.end(),
              Sort_On_Abs_Second());

    if (nfeatures != -1 && ranked.size() > nfeatures)
        ranked.erase(ranked.begin() + nfeatures, ranked.end());

    if (bias != 0.0 && includeBias)
        ranked.insert(ranked.begin(), make_pair(MISSING_FEATURE, bias));

    return ranked;
}

void
Explanation::
normalize()
{
    for (Feature_Weights::iterator it = feature_weights.begin(),
             end = feature_weights.end();
         it != end;  ++it)
        it->second = xdiv(it->second, weight);

    bias = xdiv(bias, weight);
    weight = xdiv(weight, weight);
}

/*****************************************************************************/
/* CLASSIFIER_IMPL                                                           */
/*****************************************************************************/

Classifier_Impl::Classifier_Impl()
    : label_count_(0)
{
}

namespace {

size_t get_label_count(const Feature_Space & fs,
                       const Feature & predicted)
{
    Feature_Info info = fs.info(predicted);
    return info.value_count();
}

size_t check_label_count(const Feature_Space & fs,
                         const Feature & predicted,
                         size_t label_count)
{
    /* Don't try to check if we didn't put a feature in. */
    if (predicted == MISSING_FEATURE) return label_count;

    Feature_Info info = fs.info(predicted);

    /* For reals, we assume the number of labels are known. */
    if (info.type() != REAL
        && info.value_count() != label_count) {

        throw Exception("Classifier_Impl: check_label_count(): feature ("
                        + ostream_format(info.value_count())
                        + ") and label ("
                        + ostream_format(label_count)
                        + ") counts don't match; feature = "
                        + fs.print(predicted));
    }

    return label_count;
}

} // file scope

Classifier_Impl::
Classifier_Impl(const std::shared_ptr<const Feature_Space> & feature_space,
                const Feature & predicted)
    : feature_space_(feature_space), predicted_(predicted),
      label_count_(get_label_count(*feature_space, predicted))
{
}

Classifier_Impl::
Classifier_Impl(const std::shared_ptr<const Feature_Space> & feature_space,
                const Feature & predicted,
                size_t label_count)
    : feature_space_(feature_space), predicted_(predicted),
      label_count_(check_label_count(*feature_space, predicted, label_count))
{
}

void Classifier_Impl::
init(const std::shared_ptr<const Feature_Space> & feature_space,
     const Feature & predicted)
{
    feature_space_ = feature_space;
    predicted_ = predicted;
    label_count_ = get_label_count(*feature_space, predicted);
}

void Classifier_Impl::
init(const std::shared_ptr<const Feature_Space> & feature_space,
     const Feature & predicted,
     size_t label_count)
{
    feature_space_ = feature_space;
    predicted_ = predicted;
    label_count_ = check_label_count(*feature_space, predicted, label_count);
}

Classifier_Impl::~Classifier_Impl()
{
}

float
Classifier_Impl::
predict(int label, const Feature_Set & features,
        PredictionContext * context) const
{
    if (label < 0 || label >= label_count())
        throw Exception(format("Attempt to predict non-existent label: "
                               "label = %d, label_count = %zd", label,
                               label_count()));
    return predict(features, context)[label];
}

int
Classifier_Impl::
predict_highest(const Feature_Set & features,
                PredictionContext * context) const
{
    distribution<float> prediction = predict(features, context);
    return std::max_element(prediction.begin(), prediction.end())
        - prediction.begin();
}

Optimization_Info
Classifier_Impl::
optimize(const std::vector<Feature> & features)
{
    Optimization_Info result;
    result.from_features = features;
    result.to_features = all_features();

    if (!optimization_supported())
        return result;

    map<Feature, int> & feature_map = result.feature_to_optimized_index;
    for (unsigned i = 0;  i < result.to_features.size();  ++i) {
        feature_map[result.to_features[i]] = i;
    }

    int num_done = 0;
    result.indexes.resize(features.size());
    for (unsigned i = 0;  i < features.size();  ++i) {
        if (!feature_map.count(features[i])) {
            // We don't need this feature
            result.indexes[i] = -1;
        }
        else {
            result.indexes[i] = feature_map[features[i]];
            ++num_done;
        }
    }

    if (num_done != result.to_features.size()) {
        vector<Feature> sorted1 = result.to_features;
        vector<Feature> sorted2;

        for (unsigned i = 0;  i < features.size();  ++i) {
            if (feature_map.count(features[i]))
                sorted2.push_back(features[i]);
        }

        std::sort(sorted1.begin(), sorted1.end());
        std::sort(sorted2.begin(), sorted2.end());

        vector<Feature> missing;
        std::set_difference(sorted1.begin(), sorted1.end(),
                            sorted2.begin(), sorted2.end(),
                            back_inserter(missing));

        std::string message;
        for (unsigned i = 0;  i < missing.size();  ++i) {
            if (i != 0) message += " ";
            message += feature_space()->print(missing[i]);
        }
        throw Exception("optimize(): didn't find all features needed for "
                        "classifier: %s", message.c_str());
    }

    result.initialized = true;

    optimize_impl(result);

    return result;
}

Optimization_Info
Classifier_Impl::
optimize(const Feature_Set & feature_set)
{
    if (!optimization_supported())
        return Optimization_Info();

    // Extract the list of features, and continue
    vector<Feature> features;
    features.reserve(feature_set.size());

    for (Feature_Set::const_iterator
             it = feature_set.begin(),
             end = feature_set.end();
         it != end;  ++it) {
        pair<Feature, float> val = *it;
        features.push_back(val.first);
    }

    return optimize(features);
}

bool
Classifier_Impl::
optimization_supported() const
{
    return false;
}

bool
Classifier_Impl::
predict_is_optimized() const
{
    return false;
}

Label_Dist
Classifier_Impl::
predict(const Feature_Set & features,
        const Optimization_Info & info,
        PredictionContext * context) const
{
    if (!predict_is_optimized() || !info) return predict(features, context);

    float fv[info.features_out()];
    info.apply(features, fv);

    return optimized_predict_impl(fv, info, context);
}

Label_Dist
Classifier_Impl::
predict(const std::vector<float> & features,
        const Optimization_Info & info,
        PredictionContext * context) const
{
    float fv[info.features_out()];
    info.apply(features, fv);

    return optimized_predict_impl(fv, info, context);
}

Label_Dist
Classifier_Impl::
predict(const float * features,
        const Optimization_Info & info,
        PredictionContext * context) const
{
    float fv[info.features_out()];
    info.apply(features, fv);

    return optimized_predict_impl(fv, info, context);
}

float
Classifier_Impl::
predict(int label,
        const Feature_Set & features,
        const Optimization_Info & info,
        PredictionContext * context) const
{
    if (!predict_is_optimized() || !info) return predict(label, features, context);

    float fv[info.features_out()];

    info.apply(features, fv);

    return optimized_predict_impl(label, fv, info, context);
}

float
Classifier_Impl::
predict(int label,
        const std::vector<float> & features,
        const Optimization_Info & info,
        PredictionContext * context) const
{
    if (!predict_is_optimized() || !info) {

        // Convert to standard feature set, then call classical predict
        Dense_Feature_Set fset(make_unowned_sp(info.to_features),
                               &features[0]);

        return predict(label, fset);
    }

    float fv[info.features_out()];

    info.apply(features, fv);

    return optimized_predict_impl(label, fv, info, context);
}

float
Classifier_Impl::
predict(int label,
        const float * features,
        const Optimization_Info & info,
        PredictionContext * context) const
{
    if (!predict_is_optimized() || !info) {

        // Convert to standard feature set, then call classical predict
        Dense_Feature_Set fset(make_unowned_sp(info.to_features),
                               features);

        return predict(label, fset, context);
    }

    float fv[info.features_out()];

    info.apply(features, fv);

    return optimized_predict_impl(label, fv, info, context);
}

bool
Classifier_Impl::
optimize_impl(Optimization_Info & info)
{
    return false;
}

Label_Dist
Classifier_Impl::
optimized_predict_impl(const float * features,
                       const Optimization_Info & info,
                       PredictionContext * context) const
{
    // If the classifier implemented optimized predict, then this would have
    // been overridden.

    // Convert to standard feature set, then call classical predict
    Dense_Feature_Set fset(make_unowned_sp(info.to_features),
                           features);

    return predict(fset, context);
}

void
Classifier_Impl::
optimized_predict_impl(const float * features,
                       const Optimization_Info & info,
                       double * accum,
                       double weight,
                       PredictionContext * context) const
{
    Label_Dist result = optimized_predict_impl(features, info, context);
    for (unsigned i = 0;  i < result.size();  ++i) {
        accum[i] += weight * result[i];
    }
}

float
Classifier_Impl::
optimized_predict_impl(int label,
                       const float * features,
                       const Optimization_Info & info,
                       PredictionContext * context) const
{
    // If the classifier implemented optimized predict, then this would have
    // been overridden.

    // Convert to standard feature set, then call classical predict
    Dense_Feature_Set fset(make_unowned_sp(info.to_features),
                           features);

    return predict(label, fset, context);
}

namespace {

struct Accuracy_Job_Info {
    const Training_Data & data;
    const distribution<float> & example_weights;
    const Classifier_Impl & classifier;
    const Optimization_Info & opt_info;
    Lock lock;
    double & correct;
    double & total;
    double & rmse_accum;

    Accuracy_Job_Info(const Training_Data & data,
                      const distribution<float> & example_weights,
                      const Classifier_Impl & classifier,
                      const Optimization_Info & opt_info,
                      double & correct, double & total,
                      double & rmse_accum)
        : data(data), example_weights(example_weights),
          classifier(classifier), opt_info(opt_info),
          correct(correct), total(total), rmse_accum(rmse_accum)
    {
    }

    void calc(int x_start, int x_end)
    {
        //cerr << "calculating from " << x_start << " to " << x_end << endl;

        double sub_total = 0.0, sub_correct = 0.0, sub_rmse = 0.0;

        bool regression_problem
            = classifier.feature_space()->info(classifier.predicted()).type()
            == REAL;

        for (unsigned x = x_start;  x < x_end;  ++x) {
            double w = (example_weights.empty() ? 1.0 : example_weights[x]);
            if (w == 0.0) continue;

            //cerr << "x = " << x << " w = " << w << endl;

            distribution<float> result = classifier.predict(data[x], opt_info);

            if (regression_problem) {
                float correct = data[x][classifier.predicted()];
                double error = 1.0 - correct;
                sub_rmse += w * error * error;
                sub_total += w;
            }
            else {
                Correctness c = correctness(result, classifier.predicted(),
                                            data[x]);
                sub_correct += w * c.possible * c.correct;
                sub_total += w * c.possible;
                sub_rmse += w * c.possible * c.margin * c.margin;
            }
        }

        Guard guard(lock);
        correct += sub_correct;
        total += sub_total;
        rmse_accum += sub_rmse;
    }
};

struct Accuracy_Job {
    Accuracy_Job(Accuracy_Job_Info & info,
                 int x_start, int x_end)
        : info(info), x_start(x_start), x_end(x_end)
    {
    }

    Accuracy_Job_Info & info;
    int x_start, x_end;

    void operator () () const
    {
        info.calc(x_start, x_end);
    }
};

} // file scope

std::pair<float, float>
Classifier_Impl::
accuracy(const Training_Data & data,
         const distribution<float> & example_weights,
         const Optimization_Info * opt_info_ptr) const
{
    double correct = 0.0;
    double total = 0.0;
    double rmse_accum = 0.0;

    if (!example_weights.empty()
        && example_weights.size() != data.example_count())
        throw Exception("Classifier_Impl::accuracy(): dataset and weight "
                        "vector sizes don't match");

    unsigned nx = data.example_count();

    Optimization_Info new_opt_info;
    const Optimization_Info & opt_info
        = (opt_info_ptr ? *opt_info_ptr : new_opt_info);

    Accuracy_Job_Info info(data, example_weights, *this, opt_info,
                           correct, total, rmse_accum);
    static Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB,
                                 format("accuracy group under %d", parent),
                                 parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        /* Do 1024 examples per job. */
        for (unsigned x = 0;  x < data.example_count();  x += 1024)
            worker.add(Accuracy_Job(info, x, std::min(x + 1024, nx)),
                       format("accuracy example %d to %d under %d",
                              x, x + 1024, group),
                       group);
    }

    worker.run_until_finished(group);

    return make_pair(correct / total, sqrt(rmse_accum / total));
}

namespace {

struct Predict_Job {

    int x_start, x_end;
    const Classifier_Impl & classifier;
    const Optimization_Info * opt_info;
    const Training_Data & data;

    Predict_Job(int x_start, int x_end,
                const Classifier_Impl & classifier,
                const Optimization_Info * opt_info,
                const Training_Data & data)
        : x_start(x_start), x_end(x_end),
          classifier(classifier), opt_info(opt_info),
          data(data)
    {

    }

    typedef void result_type;

    void operator () (Classifier_Impl::Predict_All_Output_Func output)
    {
        for (unsigned x = x_start;  x < x_end;  ++x) {
            Label_Dist prediction
                = opt_info
                ? classifier.predict(data[x], *opt_info)
                : classifier.predict(data[x]);
            output(x, &prediction[0]);
        }
    }

    void operator () (int label,
                      Classifier_Impl::Predict_One_Output_Func output)
    {
        for (unsigned x = x_start;  x < x_end;  ++x) {
            float prediction
                = opt_info
                ? classifier.predict(label, data[x], *opt_info)
                : classifier.predict(label, data[x]);
            output(x, prediction);
        }
    }
};

} // file scope

void
Classifier_Impl::
predict(const Training_Data & data,
        Predict_All_Output_Func output,
        const Optimization_Info * opt_info) const
{
    unsigned nx = data.example_count();

    static Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB,
                                 format("predict group under %d", parent),
                                 parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        /* Do 1024 examples per job. */
        for (unsigned x = 0;  x < data.example_count();  x += 1024)
            worker.add(boost::bind(Predict_Job(x,
                                               std::min(x + 1024, nx),
                                               *this,
                                               opt_info,
                                               data),
                                   output),
                       "predict job",
                       group);
    }

    worker.run_until_finished(group);
}

void
Classifier_Impl::
predict(const Training_Data & data,
        int label,
        Predict_One_Output_Func output,
        const Optimization_Info * opt_info) const
{
    unsigned nx = data.example_count();

    static Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB,
                                 format("predict group under %d", parent),
                                 parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        /* Do 1024 examples per job. */
        for (unsigned x = 0;  x < data.example_count();  x += 1024)
            worker.add(boost::bind(Predict_Job(x,
                                               std::min(x + 1024, nx),
                                               *this,
                                               opt_info,
                                               data),
                                   label,
                                   output),
                       "predict job",
                       group);
    }

    worker.run_until_finished(group);
}

Explanation
Classifier_Impl::
explain(const Feature_Set & feature_set,
        int label,
        double weight,
        PredictionContext * context) const
{
    throw Exception("class " + demangle(typeid(*this).name())
                    + " doesn't implement the explain() method");
}

std::shared_ptr<Classifier_Impl>
Classifier_Impl::
poly_reconstitute(DB::Store_Reader & store,
                  const std::shared_ptr<const Feature_Space> & fs)
{
    compact_size_t fs_flag(store);
    //cerr << "fs_flag = " << fs_flag << endl;
    if (fs_flag) {
        std::shared_ptr<Feature_Space> fs2;
        store >> fs2;  // ignore this one
        return Registry<Classifier_Impl>::singleton().reconstitute(store, fs);
    }
    else
        return Registry<Classifier_Impl>::singleton().reconstitute(store, fs);
}

std::shared_ptr<Classifier_Impl>
Classifier_Impl::poly_reconstitute(DB::Store_Reader & store)
{
    //cerr << __PRETTY_FUNCTION__ << endl;
    compact_size_t fs_flag(store);

    //cerr << "poly_reconstitute: fs_flag = " << fs_flag << endl;

    std::shared_ptr<const Feature_Space> fs;
    std::shared_ptr<Feature_Space> fs_mutable;
    if (fs_flag) {
        store >> fs_mutable;

        //cerr << "reconstituted feature space " << fs_mutable->print() << endl;

        fs = fs_mutable;
    }
    else fs = FS_Context::inner();

    //cerr << "reconstituting with feature space" << endl;

    std::shared_ptr<Classifier_Impl> result
        = Registry<Classifier_Impl>::singleton().reconstitute(store, fs);

    //cerr << "result->predicted() = " << result->predicted() << endl;

    //cerr << "poly_reconstitute: feature_space = " << result->feature_space()
    //     << endl;

    if (fs_mutable) {
        //cerr << "freezing mutable" << endl;
        fs_mutable->freeze();
    }

    return result;
}

void Classifier_Impl::
poly_serialize(DB::Store_Writer & store, bool write_fs) const
{
    if (write_fs) {
        store << compact_size_t(1);
        store << feature_space();
    }
    else store << compact_size_t(0);

    Registry<Classifier_Impl>::singleton().serialize(store, this);
}

bool Classifier_Impl::merge_into(const Classifier_Impl & other, float)
{
    return false;
}

Classifier_Impl *
Classifier_Impl::merge(const Classifier_Impl & other, float) const
{
    return 0;
}

std::string
Classifier_Impl::
summary() const
{
    return class_id();
}

/*****************************************************************************/
/* POLYMORPHIC STUFF                                                         */
/*****************************************************************************/

namespace {

/* Put one of these objects per thread. */
boost::thread_specific_ptr<vector<std::shared_ptr<const Feature_Space> > >
    fs_stack;

} // file scope

FS_Context::
FS_Context(const std::shared_ptr<const Feature_Space> & feature_space)
{
    if (!fs_stack.get())
        fs_stack.reset(new vector<std::shared_ptr<const Feature_Space> >());
    fs_stack->push_back(feature_space);
}

FS_Context::~FS_Context()
{
    if (!fs_stack.get())
        throw Exception("FS_Context never initialized");
    if (fs_stack->empty())
        throw Exception("FS stack was empty in destructor; bad problem");
    fs_stack->pop_back();
}

const std::shared_ptr<const Feature_Space> & FS_Context::inner()
{
    if (!fs_stack.get())
        throw Exception("FS_Context never initialized");
    if (fs_stack->empty()) throw Exception("feature space stack is empty");
    return fs_stack->back();
}

DB::Store_Writer &
operator << (DB::Store_Writer & store,
             const std::shared_ptr<const Classifier_Impl> & classifier)
{
    classifier->poly_serialize(store);
    return store;
}

DB::Store_Reader &
operator >> (DB::Store_Reader & store,
             std::shared_ptr<Classifier_Impl> & classifier)
{
    Classifier_Impl::poly_reconstitute(store, FS_Context::inner());
    return store;
}

/*****************************************************************************/
/* CLASSIFIER                                                                */
/*****************************************************************************/

Classifier::Classifier()
{
}

Classifier::Classifier(const std::shared_ptr<Classifier_Impl> & impl)
    : impl(impl)
{
}

Classifier::Classifier(Classifier_Impl * impl, bool take_copy)
{
    if (take_copy) this->impl.reset(impl->make_copy());
    else this->impl.reset(impl);
}

Classifier::Classifier(const Classifier_Impl & impl)
    : impl(impl.make_copy())
{
}

Classifier::
Classifier(const std::string & name,
           const std::shared_ptr<const Feature_Space> & feature_space)
{
    throw Exception("Classifier::Classifier(string, fs): not implemented");
    //impl = Registry<Classifier_Impl>::singleton().create(name, feature_space);
}

Classifier::
Classifier(const std::shared_ptr<const Feature_Space> & feature_space,
           DB::Store_Reader & store)
{
    impl = Classifier_Impl::poly_reconstitute(store, feature_space);
}

Classifier::Classifier(const Classifier & other)
    : impl(other.impl ? other.impl->make_copy() : 0)
{
}

Classifier &
Classifier::operator = (const Classifier & other)
{
    if (other.impl) impl.reset(other.impl->make_copy());
    else impl.reset();
    return *this;
}

DB::Store_Writer &
operator << (DB::Store_Writer & store, const Classifier & classifier)
{
    classifier.serialize(store);
    return store;
}

DB::Store_Reader &
operator >> (DB::Store_Reader & store, Classifier & classifier)
{
    classifier.reconstitute(store, FS_Context::inner());
    return store;
}

void Classifier::load(const std::string & filename)
{
    filter_istream stream(filename);
    Store_Reader store(stream);
    reconstitute(store);
}

void Classifier::
load(const std::string & filename, std::shared_ptr<const Feature_Space> fs)
{
    filter_istream stream(filename);
    Store_Reader store(filename);
    reconstitute(store, fs);
}

void Classifier::save(const std::string & filename, bool write_fs) const
{
    Store_Writer store(filename);
    serialize(store, write_fs);
}

/*****************************************************************************/
/* FACTORY_BASE<CLASSIFIER_IMPL>                                             */
/*****************************************************************************/

Factory_Base<Classifier_Impl>::~Factory_Base()
{
}

} // namespace ML


#include "Scene.h"

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
namespace pt = boost::property_tree;

using namespace GLCore;
using namespace GLCore::Utils;

SceneNode::SceneNode(const std::string& name, const Mesh& mesh, const Material& material, const glm::mat4& modelMatrix)
	: m_Name(name), m_Mesh(mesh), m_Material(material), m_ModelMatrix(modelMatrix)
{
	m_invModelMatrix = glm::inverse(m_ModelMatrix);
}

SceneNode::SceneNode(const SceneNode& node)
	: m_Name(node.m_Name), m_Mesh(node.m_Mesh), m_Material(node.m_Material), m_ModelMatrix(node.m_ModelMatrix), m_invModelMatrix(node.m_invModelMatrix)
{
}

SceneNode::~SceneNode()
{
}

void SceneNode::Setup()
{
	m_Mesh.Setup();
}

void SceneNode::Draw(GLCore::Utils::Shader* shader)
{
	int location = glGetUniformLocation(shader->GetRendererID(), "u_Model");
	glUniformMatrix4fv(location, 1, GL_FALSE, glm::value_ptr(m_ModelMatrix));
	m_Mesh.Draw();
}

Scene::Scene(const std::string& filepath)
	: m_Camera(45.0f, 16.0f / 9.0f, 0.01f, 1000.0f)
{
	Scene::LoadSceneScript(filepath);
}

Scene::~Scene()
{
}

void Scene::Setup()
{
	for (auto& it : m_Nodes)
	{
		it.Setup();
	}
}

void Scene::Draw(GLCore::Utils::Shader* shader)
{
	for (auto& it : m_Nodes)
	{
		it.Draw(shader);
	}
}

void Scene::LoadSceneScript(const std::string& filepath)
{
	LOG_INFO("[SCENE] Loading scene!");
	pt::ptree root;
	pt::read_json(filepath, root);

	LOG_INFO("[SCENE] Loading camera!");
	std::vector<float> aspect;

	m_fovy = root.get<float>("camera.fovy");
	for (pt::ptree::value_type& value : root.get_child("camera.aspect"))
	{
		aspect.push_back(value.second.get_value<float>());
	}
	m_aspect = aspect[0] / aspect[1];
	m_zNear = root.get<float>("camera.zNear");
	m_zFar = root.get<float>("camera.zFar");
	m_Camera.SetProjection(m_fovy, m_aspect, m_zNear, m_zFar);

	LOG_INFO("[SCENE] Loading light sources!");
	for (pt::ptree::value_type& light : root.get_child("light_sources"))
	{
		LOG_INFO("[SCENE::LIGHT] Loading light!");

		std::vector<float> color;
		for (pt::ptree::value_type& value : light.second.get_child("color"))
		{
			color.push_back(value.second.get_value<float>());
		}

		std::vector<float> position;
		for (pt::ptree::value_type& value : light.second.get_child("position"))
		{
			position.push_back(value.second.get_value<float>());
		}

		float constant = light.second.get<float>("constant");
		float linear = light.second.get<float>("linear");
		float quadratic = light.second.get<float>("quadratic");

		m_Lights.push_back(PointLight(glm::vec3(color[0], color[1], color[2]), glm::vec3(position[0], position[1], position[2]), constant, linear, quadratic));
	}

	LOG_INFO("[SCENE] Loading materials!");
	std::unordered_map<std::string, Material> materials;
	for (pt::ptree::value_type& material : root.get_child("materials"))
	{
		LOG_INFO("[SCENE::MATERIAL] Loading material!");
		std::string name = material.first.data();

		std::vector<float> ambient;
		for (pt::ptree::value_type& value : material.second.get_child("ambient"))
		{
			ambient.push_back(value.second.get_value<float>());
		}

		std::vector<float> diffuse;
		for (pt::ptree::value_type& value : material.second.get_child("diffuse"))
		{
			diffuse.push_back(value.second.get_value<float>());
		}

		std::vector<float> specular;
		for (pt::ptree::value_type& value : material.second.get_child("specular"))
		{
			specular.push_back(value.second.get_value<float>());
		}

		float shininess = material.second.get<float>("shininess");

		materials.insert(std::make_pair(name, Material(glm::vec3(ambient[0], ambient[1], ambient[2]), glm::vec3(diffuse[0], diffuse[1], diffuse[2]), glm::vec3(specular[0], specular[1], specular[2]), shininess)));
	}

	LOG_INFO("[SCENE] Loading objects!");
	for (pt::ptree::value_type& object : root.get_child("objects"))
	{
		LOG_INFO("[SCENE::OBJECTS] Loading object!");
		std::string name = object.first;

		std::vector<float> position;
		for (pt::ptree::value_type& value : object.second.get_child("position"))
		{
			position.push_back(value.second.get_value<float>());
		}
		glm::mat4 translationMatrix = glm::translate(glm::mat4(1.0f), glm::vec3(position[0], position[1], position[2]));

		std::vector<float> rotation;
		for (pt::ptree::value_type& value : object.second.get_child("rotation"))
		{
			rotation.push_back(value.second.get_value<float>());
		}
		glm::vec3 EulerAngles(rotation[0], rotation[1], rotation[2]);
		glm::mat4 rotationMatrix = glm::toMat4(glm::quat(EulerAngles));

		std::vector<float> scale;
		for (pt::ptree::value_type& value : object.second.get_child("scale"))
		{
			scale.push_back(value.second.get_value<float>());
		}
		glm::mat4 scaleMatrix = glm::scale(glm::mat4(1.0f), glm::vec3(scale[0], scale[1], scale[2]));

		glm::mat4 modelMatrix = translationMatrix * rotationMatrix * scaleMatrix;

		std::string material = object.second.get<std::string>("material");

		std::string buffer;
		bool isPrimitive = object.second.get<bool>("primitive");
		if (isPrimitive)
		{
			buffer = object.second.get<std::string>("type");
			if (!buffer.compare("cube"))
			{
				auto it = materials.find(material);
				if (it == materials.end())
				{
					material = "Material '" + material + "' not found.";
					LOG_WARN(material.c_str());
				}
				else
				{
					m_Nodes.push_back(SceneNode(name, Mesh(Cube()), it->second, modelMatrix));
				}
			}
			else if (!buffer.compare("plane"))
			{
				auto it = materials.find(material);
				if (it == materials.end())
				{
					material = "Material '" + material + "' not found.";
					LOG_WARN(material.c_str());
				}
				else
				{
					m_Nodes.push_back(SceneNode(name, Mesh(Plane()), it->second, modelMatrix));
				}
			}
			else
			{
				buffer = "The primitive '" + buffer + "' is not defined";
				LOG_WARN(buffer.c_str());
			}
		}
		else
		{
			buffer = object.second.get<std::string>("filepath");

			auto it = materials.find(material);
			if (it == materials.end())
			{
				material = "Material '" + material + "' not found.";
				LOG_WARN(material.c_str());
			}
			else
			{
				m_Nodes.push_back(SceneNode(name, Mesh(buffer), it->second, modelMatrix));
			}
		}
	}

	LOG_INFO("[SCENE] Scene loaded!");
}


// Copyright Stephan T. Lavavej, http://nuwen.net .
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENSE_1_0.txt or copy at
// http://boost.org/LICENSE_1_0.txt .

#ifndef PHAM_MTF_HH
#define PHAM_MTF_HH

#include "compiler.hh"

#ifdef NUWEN_PLATFORM_MSVC
    #pragma once
#endif

#include "typedef.hh"

#include "external_begin.hh"
    #include <algorithm>
    #include <boost/utility.hpp>
#include "external_end.hh"

namespace nuwen {
    inline void   mtf2(vuc_t& v);
    inline void unmtf2(vuc_t& v);
}

namespace pham {
    namespace mtf {
        class state : public boost::noncopyable {
        public:
            state() : m_lastposzero(true) {
                for (int i = 0; i < 256; ++i) {
                    m_l[i] = static_cast<nuwen::uc_t>(i);
                }
            }

            nuwen::uc_t get_pos(const nuwen::uc_t byte) const {
                nuwen::uc_t pos = 0;

                while (m_l[pos] != byte) {
                    ++pos;
                }

                return pos;
            }

            nuwen::uc_t get_byte(const nuwen::uc_t pos) const {
                return m_l[pos];
            }

            void update(const nuwen::uc_t byte, const nuwen::uc_t pos) {
                switch (pos) {
                    case 0:
                        m_lastposzero = true;
                        break;
                    case 1:
                        if (!m_lastposzero) {
                            m_l[1] = m_l[0];
                            m_l[0] = byte;
                        }

                        m_lastposzero = false;
                        break;
                    default:
                        std::copy_backward(m_l + 1, m_l + pos, m_l + pos + 1);
                        m_l[1] = byte;
                        m_lastposzero = false;
                        break;
                }
            }

        private:
            nuwen::uc_t m_l[256];
            bool m_lastposzero;
        };
    }
}

inline void nuwen::mtf2(vuc_t& v) {
    pham::mtf::state s;

    for (vuc_i_t i = v.begin(); i != v.end(); ++i) {
        const uc_t byte = *i;
        const uc_t pos = s.get_pos(byte);

        s.update(byte, pos);

        *i = pos;
    }
}

inline void nuwen::unmtf2(vuc_t& v) {
    pham::mtf::state s;

    for (vuc_i_t i = v.begin(); i != v.end(); ++i) {
        const uc_t pos = *i;
        const uc_t byte = s.get_byte(pos);

        s.update(byte, pos);

        *i = byte;
    }
}

#endif // Idempotency


#ifndef PLANNING_COSTMAP_HPP
#define PLANNING_COSTMAP_HPP

// Component
#include "Point.hpp"
#include "RosConversionHelper.hpp"

// Ros
#include <geometry_msgs/Pose.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>

// Libraries
#include <boost/cstdfloat.hpp>
#include <Eigen/Dense>

// Standard
#include <cstdint>

namespace local_planner
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class to store information about occupied cells
class Costmap
{
public:
    /// @brief Mutator for setting the occupancy grid
    /// @param grid The occupancy grid
    /// @param pose The local pose of the point
    inline void setOccupancyGrid(const nav_msgs::OccupancyGrid::ConstPtr& grid, const geometry_msgs::Pose& pose)
    {
        m_grid = grid;
        m_local_pose = pose;
    }

    inline std::int64_t getProbabilityOccupied(const Point& pt)
    {
        const float64_t resolution  = m_grid->info.resolution;
        const float64_t width       = m_grid->info.width;
        const float64_t heading_r   = RosConversionHelper::quaternionMsgToYawR(m_local_pose.orientation);
        const float64_t base_x_m    =  (pt.getX() - m_local_pose.position.x)*std::cos(heading_r) + (pt.getY() - m_local_pose.position.y)*std::sin(heading_r);
        const float64_t base_y_m    = -(pt.getX() - m_local_pose.position.x)*std::sin(heading_r) + (pt.getY() - m_local_pose.position.y)*std::cos(heading_r);

        const std::size_t x = static_cast<std::size_t>((base_x_m - m_grid->info.origin.position.x)/resolution);
        const std::size_t y = static_cast<std::size_t>((base_y_m - m_grid->info.origin.position.y)/resolution);

        const std::size_t grid_it = x + y*static_cast<std::size_t>(width);

        if (grid_it > m_grid->data.size())
        {
            return 0;
        }

        return static_cast<std::int64_t>(m_grid->data[grid_it]);
    }

private:
    nav_msgs::OccupancyGrid::ConstPtr       m_grid;        ///< Occupancy Grid
    geometry_msgs::Pose                     m_local_pose;  ///< Local Pose
};

} // namespace local_planner

#endif // PLANNING_COSTMAP_HPP


/** $Id: SimplePropagator.cxx 150208 2016-09-21 11:32:47Z tfuchs $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 150208 $
 * $Date: 2016-09-21 13:32:47 +0200 (Mi, 21. Sep 2016) $
 */

#include <boost/bind.hpp>
#include <vector>

#include "PROPOSAL-icetray/Converter.h"
#include "PROPOSAL-icetray/SimplePropagator.h"

using namespace PROPOSAL;

SimplePropagator::SimplePropagator(I3Particle::ParticleType pt,
                                   const std::string& medium,
                                   double ecut,
                                   double vcut,
                                   double rho,
                                   I3Particle::ParticleType final_loss)
    : propagator_(NULL)
    , final_stochastic_loss_(final_loss)
{
    std::ostringstream prefix;
    prefix << getenv("I3_BUILD") << "/MuonGun/resources/tables";

    // Sector definition

    PROPOSAL::Sector::Definition sec_def;

    sec_def.stopping_decay              = final_stochastic_loss_ == I3Particle::unknown;
    sec_def.scattering_model            = PROPOSAL::ScatteringFactory::Highland;
    sec_def.do_exact_time_calculation   = true;
    sec_def.do_continuous_randomization = ecut < 0;

    // Medium

    sec_def.SetMedium(*PROPOSAL::MediumFactory::Get().CreateMedium(medium, rho));

    // Geometry

    PROPOSAL::Vector3D position(0.0, 0.0, 0.0);
    PROPOSAL::Sphere detector(position, 1e18, 0.0);

    sec_def.SetGeometry(detector);

    // Cuts

    sec_def.cut_settings.SetEcut(ecut);
    sec_def.cut_settings.SetVcut(vcut);

    std::vector<Sector::Definition> sec_def_vec;
    sec_def_vec.push_back(sec_def);

    // Parametrizations

    // Using defaults:
    // lpm effect = true
    // Bremsstrahlung = KelnerKokoulinPetrukhin
    // Photonuclear = AbramowiczLevinLevyMaor97

    // Interpolation

    PROPOSAL::InterpolationDef interpolation_def;
    interpolation_def.path_to_tables = prefix.str();
    interpolation_def.path_to_tables_readonly = prefix.str();

    // Init new propagator

    propagator_ = new PROPOSAL::Propagator(particle_converter_.GeneratePROPOSALType(pt),
                                           sec_def_vec,
                                           detector,
                                           interpolation_def);
}

SimplePropagator::~SimplePropagator()
{
    delete propagator_;
}

void SimplePropagator::SetSeed(int seed)
{
    PROPOSAL::RandomGenerator::Get().SetSeed(seed);
}

void SimplePropagator::SetRandomNumberGenerator(I3RandomServicePtr rng)
{
    std::function<double()> f = boost::bind(&I3RandomService::Uniform, rng, 0, 1);
    PROPOSAL::RandomGenerator::Get().SetRandomNumberGenerator(f);
}

I3Particle SimplePropagator::propagate(const I3Particle& p,
                                       double distance,
                                       boost::shared_ptr<std::vector<I3Particle> > losses)
{
    I3Particle endpoint(p);

    double x = p.GetPos().GetX() / I3Units::cm;
    double y = p.GetPos().GetY() / I3Units::cm;
    double z = p.GetPos().GetZ() / I3Units::cm;

    double theta = p.GetDir().CalcTheta() / I3Units::radian;
    double phi   = p.GetDir().CalcPhi() / I3Units::radian;

    double energy = p.GetEnergy() / I3Units::MeV;
    double time   = p.GetTime() / I3Units::second;
    double length = p.GetLength() / I3Units::cm;

    // The Muons from NuGen have NaN as default propagated length.
    // So this has to be corrected.
    if (std::isnan(length))
    {
        length = 0.0;
    }
    else if (std::isinf(length))
    {
        log_fatal("the propagated length is Inf, should be finite or NaN.");
    }

    PROPOSAL::Particle& particle = propagator_->GetParticle();
    particle.SetParentParticleId(0);
    particle.SetParticleId(0);

    particle.SetPosition(PROPOSAL::Vector3D(x, y, z));

    PROPOSAL::Vector3D direction;
    direction.SetSphericalCoordinates(1.0, phi, theta);
    direction.CalculateCartesianFromSpherical();
    particle.SetDirection(direction);

    particle.SetEnergy(energy);
    particle.SetTime(time);
    particle.SetPropagatedDistance(length);

    std::vector<PROPOSAL::DynamicData*> history = propagator_->Propagate(distance / I3Units::cm);

    x = particle.GetPosition().GetX() * I3Units::cm;
    y = particle.GetPosition().GetY() * I3Units::cm;
    z = particle.GetPosition().GetZ() * I3Units::cm;

    theta = particle.GetDirection().GetTheta() * I3Units::radian;
    phi   = particle.GetDirection().GetPhi() * I3Units::radian;

    energy = particle.GetEnergy() * I3Units::MeV;
    time   = particle.GetTime() * I3Units::second;
    length = particle.GetPropagatedDistance() * I3Units::cm;

    endpoint.SetPos(x, y, z);
    endpoint.SetThetaPhi(theta, phi);

    endpoint.SetEnergy(energy);
    endpoint.SetLength(length);
    endpoint.SetTime(time);

    // Tomasz
    if (losses)
    {
        // std::vector<PROPOSAL::DynamicData*> history = Output::getInstance().GetSecondarys();
        for (unsigned int i = 0; i < history.size(); i++)
        {
            losses->push_back(particle_converter_.GenerateI3Particle(*history[i]));
        }

        if (final_stochastic_loss_ != I3Particle::unknown)
        {
            I3Particle i3_particle = particle_converter_.GenerateI3Particle(particle);
            i3_particle.SetType(final_stochastic_loss_);
            i3_particle.SetEnergy(particle.GetEnergy() - particle.GetParticleDef().mass);

            losses->push_back(i3_particle);
        }
    }

    Output::getInstance().ClearSecondaryVector();

    return endpoint;
}


#ifndef PREQUEL_ENGINE_BLOCK_MAP_HPP
#define PREQUEL_ENGINE_BLOCK_MAP_HPP

#include "base.hpp"
#include "block.hpp"

#include <prequel/math.hpp>

#include <boost/intrusive/unordered_set.hpp>

#include <algorithm>
#include <vector>

namespace prequel::detail::engine_impl {

/// A map that indexes all live block instances.
class block_map {
public:
    /// Constructs a block map with the given size hint.
    ///
    /// \param expected_load
    ///     The approximate expected worst case load.
    ///     This number is used to compute an appropriate
    ///     number of hash table buckets.
    explicit block_map(size_t expected_load)
        : m_buckets(round_towards_pow2(std::max(size_t(32), (expected_load * 4) / 3)))
        , m_map(map_t::bucket_traits(m_buckets.data(), m_buckets.size())) {}

    ~block_map() = default;

    auto begin() noexcept { return m_map.begin(); }

    auto end() noexcept { return m_map.end(); }

    void clear() noexcept { m_map.clear(); }

    template<typename Disposer>
    void dispose(Disposer&& d) {
        m_map.clear_and_dispose(std::forward<Disposer>(d));
    }

    /// Insert a block into the map.
    ///
    /// \pre The block must not be stored already.
    /// \pre The block's index must be unique.
    void insert(block* blk) noexcept {
        PREQUEL_ASSERT(blk, "Invalid block pointer.");
        PREQUEL_ASSERT(!contains(blk), "Block is already stored in a map.");

        bool inserted;
        std::tie(std::ignore, inserted) = m_map.insert(*blk);
        PREQUEL_ASSERT(inserted, "A block with that index already exists.");
    }

    /// Removes a block from the map.
    ///
    /// \pre `contains(blk)`.
    void remove(block* blk) noexcept {
        PREQUEL_ASSERT(blk, "Invalid block pointer.");
        PREQUEL_ASSERT(contains(blk), "block not stored in map.");

        m_map.erase(m_map.iterator_to(*blk));
    }

    /// Finds the block with the given index and returns it.
    /// Returns nullptr if no such block was found.
    block* find(u64 index) const noexcept {
        auto iter = m_map.find(index);
        if (iter == m_map.end()) {
            return nullptr;
        }

        const block& blk = *iter;
        return const_cast<block*>(&blk);
    }

    /// Returns true if the block is inside this map.
    bool contains(block* blk) const noexcept {
        PREQUEL_ASSERT(blk, "Invalid block pointer.");
        return blk->m_map_hook.is_linked();
    }

    /// Returns the number of blocks in this map.
    size_t size() const noexcept { return m_map.size(); }

    block_map(const block_map&) = delete;
    block_map& operator=(const block_map&) = delete;

private:
    struct block_index_hash {
        size_t operator()(u64 index) const noexcept { return boost::hash_value(index); }
    };

    using map_t = boost::intrusive::unordered_set<
        block,
        boost::intrusive::member_hook<block, boost::intrusive::unordered_set_member_hook<>,
                                      &block::m_map_hook>,
        boost::intrusive::power_2_buckets<true>, boost::intrusive::key_of_value<index_of_block>,
        boost::intrusive::hash<block_index_hash>>;

    using bucket_t = typename map_t::bucket_type;

private:
    /// Holds the buckets for the intrusive map.
    std::vector<bucket_t> m_buckets;

    /// Maps block indices to block instances.
    map_t m_map;
};

} // namespace prequel::detail::engine_impl

#endif // PREQUEL_ENGINE_BLOCK_MAP_HPP


/* (c) 2016, 2021 FFF Services. For details refers to LICENSE.txt */

#include <fc/io/json.hpp>
#include <fc/smart_ref_impl.hpp>
#include <fc/variant.hpp>

#include <fc/variant_object.hpp>
#include <fc/crypto/hex.hpp>

#include <graphene/chain/protocol/operations.hpp>

#include <algorithm>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>
#include <boost/algorithm/string.hpp>

using namespace graphene::chain;

using namespace std;
using namespace fc;
namespace bpo = boost::program_options;

fc::ecc::private_key derive_private_key( const std::string& prefix_string,
                                         int sequence_number )
{
   std::string sequence_string = std::to_string(sequence_number);
   fc::sha512 h = fc::sha512::hash(prefix_string + " " + sequence_string);
   fc::ecc::private_key derived_key = fc::ecc::private_key::regenerate(fc::sha256::hash(h));
   return derived_key;
}

string normalize_brain_key( string s )
{
   size_t i = 0, n = s.length();
   std::string result;
   char c;
   result.reserve( n );

   bool preceded_by_whitespace = false;
   bool non_empty = false;
   while( i < n )
   {
      c = s[i++];
      switch( c )
      {
      case ' ':  case '\t': case '\r': case '\n': case '\v': case '\f':
         preceded_by_whitespace = true;
         continue;

      case 'a': c = 'A'; break;
      case 'b': c = 'B'; break;
      case 'c': c = 'C'; break;
      case 'd': c = 'D'; break;
      case 'e': c = 'E'; break;
      case 'f': c = 'F'; break;
      case 'g': c = 'G'; break;
      case 'h': c = 'H'; break;
      case 'i': c = 'I'; break;
      case 'j': c = 'J'; break;
      case 'k': c = 'K'; break;
      case 'l': c = 'L'; break;
      case 'm': c = 'M'; break;
      case 'n': c = 'N'; break;
      case 'o': c = 'O'; break;
      case 'p': c = 'P'; break;
      case 'q': c = 'Q'; break;
      case 'r': c = 'R'; break;
      case 's': c = 'S'; break;
      case 't': c = 'T'; break;
      case 'u': c = 'U'; break;
      case 'v': c = 'V'; break;
      case 'w': c = 'W'; break;
      case 'x': c = 'X'; break;
      case 'y': c = 'Y'; break;
      case 'z': c = 'Z'; break;

      default:
         break;
      }
      if( preceded_by_whitespace && non_empty )
         result.push_back(' ');
      result.push_back(c);
      preceded_by_whitespace = false;
      non_empty = true;
   }
   return result;
}

int main( int argc, char** argv )
{
    try {
        boost::program_options::options_description opts;
        opts.add_options()
        ("verify", "Verify the signature" )
        ("sign", "Sign the buffer" )
        ("buffer", bpo::value<string>()->default_value(""), "Buffer to verify or sign.")
        ("brainkey", bpo::value<string>()->default_value(""), "Brain key.")
        ("publickey", bpo::value<string>()->default_value(""), "Public key.")
        ("signature", bpo::value<string>()->default_value(""), "Signature to verify.")
        ;

        bpo::variables_map options;
        bpo::store( bpo::parse_command_line(argc, argv, opts), options );

        if (options.count( "verify" ) && options.count( "sign" ) ) {
            std::cerr << "Either sign or verify please" << std::endl;
            return 1;
        }

        std::string str_buffer = options.at("buffer").as<string>();
        std::string str_brainkey = options.at("brainkey").as<string>();
        std::string str_publickey = options.at("publickey").as<string>();
        std::string str_signature = options.at("signature").as<string>();

        if ( options.count( "sign" ) ) {
            if (str_buffer.size() == 0 || str_brainkey.size() == 0) {
                std::cerr << "You need buffer and brainkey to sign" << std::endl;
                return 1;
            }

            string normalized_brain_key = normalize_brain_key( str_brainkey );
            fc::ecc::private_key privkey = derive_private_key( normalized_brain_key, 0 );

            sha256 digest(str_buffer);

            auto sign = privkey.sign_compact(digest);

           std::cout << to_hex((const char*)sign.begin(), sign.size()) << std::endl;

        } else {
            if (str_buffer.size() == 0 || str_publickey.size() == 0 || str_signature.size() == 0) {
                std::cerr << "You need buffer, public key and signature to verify" << std::endl;
                return 1;
            }

           fc::ecc::compact_signature signature;
           from_hex(str_signature, (char*)signature.begin(), signature.size());
           sha256 digest(str_buffer);

           fc::ecc::public_key pub_key(signature, digest);
           public_key_type provided_key(str_publickey);

           if (provided_key == pub_key) {
              cout << "Valid" << std::endl;
              return 0;
           } else {
              cerr << "Invalid signature" << std::endl;
              return 1;
           }

        }

    }
    catch ( const fc::exception& e ) {
    }

   return 0;
}


// This file is part of SWGANH which is released under the MIT license.
// See file LICENSE or go to http://swganh.com/LICENSE

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE Spatial Index Test
#include <boost/test/unit_test.hpp>

#include <boost/random.hpp>

#include "node.h"
#include "swganh_core/object/object.h"

#include <boost/foreach.hpp>
#include <boost/geometry.hpp>

using namespace quadtree;

///
class NodeTest
{
public:
    NodeTest()
        : root_node_(ROOT, Region(Point(-8300.0f, -8300.0f), Point(8300.0f, 8300.0f)), 0, 9, nullptr)
        , event_dispatcher_(io_service_)
    {}

    ~NodeTest()
    {}

protected:
    virtual void SetUp() { }

    Node root_node_;
    swganh::EventDispatcher event_dispatcher_;
    boost::asio::io_service io_service_;
};

BOOST_FIXTURE_TEST_SUITE(SpatialIndexNode, NodeTest)
///
BOOST_AUTO_TEST_CASE(CanInsertRemoveObject)
{
    std::shared_ptr<swganh::object::Object> obj(new swganh::object::Object());
    obj->SetEventDispatcher(&event_dispatcher_);
    obj->SetPosition(glm::vec3(10.0f, 10.0f, 10.0f));

    root_node_.InsertObject(obj);
    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetContainedObjects().size());

    root_node_.RemoveObject(obj);
    BOOST_CHECK_EQUAL(size_t(0), root_node_.GetContainedObjects().size());
}

///
BOOST_AUTO_TEST_CASE(VerifyQuadrantSplit)
{
    std::shared_ptr<swganh::object::Object> obj1(new swganh::object::Object()), obj2(new swganh::object::Object()), obj3(new swganh::object::Object()), obj4(new swganh::object::Object());

    obj1->SetEventDispatcher(&event_dispatcher_);
    obj2->SetEventDispatcher(&event_dispatcher_);
    obj3->SetEventDispatcher(&event_dispatcher_);
    obj4->SetEventDispatcher(&event_dispatcher_);

    obj1->SetObjectId(1);
    obj2->SetObjectId(2);
    obj3->SetObjectId(3);
    obj4->SetObjectId(4);

    obj1->SetPosition(glm::vec3(-10.0f, 0.0f, 10.0f));
    obj2->SetPosition(glm::vec3(10.0f, 0.0f, 10.0f));
    obj3->SetPosition(glm::vec3(-10.0f, 0.0f, -10.0f));
    obj4->SetPosition(glm::vec3(10.0f, 0.0f, -10.0f));

    root_node_.InsertObject(obj1);
    root_node_.InsertObject(obj2);
    root_node_.InsertObject(obj3);
    root_node_.InsertObject(obj4);

    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetLeafNodes()[NW_QUADRANT]->GetObjects().size());
    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetLeafNodes()[NE_QUADRANT]->GetObjects().size());
    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetLeafNodes()[SW_QUADRANT]->GetObjects().size());
    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetLeafNodes()[SE_QUADRANT]->GetObjects().size());

    BOOST_CHECK_EQUAL(uint64_t(1), (*root_node_.GetLeafNodes()[NW_QUADRANT]->GetObjects().begin())->GetObjectId());
    BOOST_CHECK_EQUAL(uint64_t(2), (*root_node_.GetLeafNodes()[NE_QUADRANT]->GetObjects().begin())->GetObjectId());
    BOOST_CHECK_EQUAL(uint64_t(3), (*root_node_.GetLeafNodes()[SW_QUADRANT]->GetObjects().begin())->GetObjectId());
    BOOST_CHECK_EQUAL(uint64_t(4), (*root_node_.GetLeafNodes()[SE_QUADRANT]->GetObjects().begin())->GetObjectId());

    root_node_.RemoveObject(obj1);
    root_node_.RemoveObject(obj2);
    root_node_.RemoveObject(obj3);
    root_node_.RemoveObject(obj4);

    // TODO: Check that all LeafNodes have been disposed of.
}

///
BOOST_AUTO_TEST_CASE(CanQuery)
{
    std::shared_ptr<swganh::object::Object> obj(new swganh::object::Object());
    obj->SetObjectId(0xDEADBABE);
    obj->SetEventDispatcher(&event_dispatcher_);
    obj->SetPosition(glm::vec3(10.0f, 0.0f, 10.0f));

    root_node_.InsertObject(obj);

    BOOST_CHECK_EQUAL(size_t(1), root_node_.Query(QueryBox( Point(0.0f, 0.0f), Point(15.0f, 15.0f) )).size());

    root_node_.RemoveObject(obj);

    BOOST_CHECK_EQUAL(size_t(0), root_node_.Query(QueryBox( Point(0.0f, 0.0f), Point(15.0f, 15.0f) )).size());
}

///
BOOST_AUTO_TEST_CASE(CanUpdateObject)
{
    std::shared_ptr<swganh::object::Object> obj(new swganh::object::Object());
    obj->SetObjectId(0xDEADBABE);
    obj->SetEventDispatcher(&event_dispatcher_);
    obj->SetPosition(glm::vec3(10.0f, 0.0f, 10.0f));

    root_node_.InsertObject(obj);

    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetLeafNodes()[NE_QUADRANT]->GetObjects().size());
    BOOST_CHECK_EQUAL(size_t(0), root_node_.GetLeafNodes()[SE_QUADRANT]->GetObjects().size());

    glm::vec3 new_position(10.0f, 0.0f, -10.0f);
    auto old_bounding_volume = obj->GetAABB();
    obj->SetPosition(new_position);
    root_node_.UpdateObject(obj, old_bounding_volume, obj->GetAABB());

    BOOST_CHECK_EQUAL(size_t(0), root_node_.GetLeafNodes()[NE_QUADRANT]->GetObjects().size());
    BOOST_CHECK_EQUAL(size_t(1), root_node_.GetLeafNodes()[SE_QUADRANT]->GetObjects().size());

    root_node_.RemoveObject(obj);
}

///
BOOST_AUTO_TEST_CASE(CanInsertRemoveQueryOneThousand)
{
    std::vector<std::shared_ptr<swganh::object::Object>> objects;
    boost::random::mt19937 gen;
    boost::random::uniform_real_distribution<> random_generator(-3000.0f, 3000.0f);
    boost::random::uniform_int_distribution<> id_generator(1000, 1000000000);

    for(int i = 0; i < 1000; i++)
    {
        objects.push_back(std::make_shared<swganh::object::Object>());
        objects[i]->SetObjectId(id_generator(gen));
        objects[i]->SetEventDispatcher(&event_dispatcher_);
        objects[i]->SetPosition(glm::vec3(random_generator(gen), 0.0f, random_generator(gen)));
        root_node_.InsertObject(objects[i]);
    }

    BOOST_CHECK_EQUAL(size_t(1000), root_node_.GetContainedObjects().size());
    BOOST_CHECK_EQUAL(size_t(1000), root_node_.Query(QueryBox(Point(-3000, -3000), Point(3000, 3000))).size());

    for(int i = 0; i < 1000; i++)
    {
        root_node_.RemoveObject(objects[i]);
    }

    BOOST_CHECK_EQUAL(size_t(0), root_node_.GetContainedObjects().size());
    BOOST_CHECK_EQUAL(size_t(0), root_node_.Query(QueryBox(Point(-3000, -3000), Point(3000, 3000))).size());
}

///
BOOST_AUTO_TEST_CASE(CanInsertRemoveQueryTenThousand)
{
    std::vector<std::shared_ptr<swganh::object::Object>> objects;
    boost::random::mt19937 gen;
    boost::random::uniform_real_distribution<> random_generator(-3000.0f, 3000.0f);
    boost::random::uniform_int_distribution<> id_generator(1000, 1000000000);

    for(int i = 0; i < 10000; i++)
    {
        objects.push_back(std::make_shared<swganh::object::Object>());
        objects[i]->SetObjectId(id_generator(gen));
        objects[i]->SetEventDispatcher(&event_dispatcher_);
        objects[i]->SetPosition(glm::vec3(random_generator(gen), 0.0f, random_generator(gen)));
        root_node_.InsertObject(objects[i]);
    }

    BOOST_CHECK_EQUAL(size_t(10000), root_node_.GetContainedObjects().size());
    BOOST_CHECK_EQUAL(size_t(10000), root_node_.Query(QueryBox(Point(-3000, -3000), Point(3000, 3000))).size());

    for(int i = 0; i < 10000; i++)
    {
        root_node_.RemoveObject(objects[i]);
    }

    BOOST_CHECK_EQUAL(size_t(0), root_node_.GetContainedObjects().size());
    BOOST_CHECK_EQUAL(size_t(0), root_node_.Query(QueryBox(Point(-3000, -3000), Point(3000, 3000))).size());
}

BOOST_AUTO_TEST_SUITE_END()
/*****************************************************************************/