#ifndef I_SEE_PEE_I_SEE_PEE_HPP
#define I_SEE_PEE_I_SEE_PEE_HPP

#include <bits/stdc++.h>

#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>

#include <ros/ros.h>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2/convert.h>

#include <Eigen/Dense>

namespace i_see_pee {

using index_t = size_t;
using transform_t = Eigen::Affine2f;

using stamp_rep_t = unsigned long;
using stamp_t = std::chrono::duration<stamp_rep_t, std::nano>;

namespace internal {

using transform_t__ = geometry_msgs::TransformStamped;
using pose_t__ = geometry_msgs::Pose;
using quaternion_t__ = geometry_msgs::Quaternion;

using stamp_rep_t__ = unsigned int;
using stamp_t__ = ros::Time;

inline bool is_valid(const quaternion_t__& _q) noexcept {
  // define a vector in order to check if the length of the quaternion is
  // smaller than 1. Double is required since geometry_msgs uses double as
  // default floating point type.
  using vector_t = Eigen::Matrix<double, 4ul, 1ul>;
  vector_t q(_q.w, _q.x, _q.y, _q.z);
  return q.norm() <= 1.;
}

inline stamp_t to_stamp(const stamp_t__ &_t) noexcept {
  // ros time stamp stores seconds and nanoseconds in two different fields
  // we return a nanosecond duration representing both
  const std::chrono::seconds s(_t.sec);
  const std::chrono::nanoseconds n(_t.nsec);
  return s + n;
}

inline stamp_t__ to_stamp(const stamp_t &_t) noexcept {
  // pull the std::chrono namespace to maintain readability
  using namespace std::chrono;
  // cast the stamp first to seconds and subtract them from the full stamp to
  // retrieve also the nanoseconds.
  const auto s = duration_cast<seconds, stamp_rep_t>(_t);
  const auto n = _t - s;
  stamp_t__ out(static_cast<stamp_rep_t__>(s.count()),
                static_cast<stamp_rep_t__>(n.count()));

  return out;
}

transform_t to_transform(const transform_t__ &_tf) noexcept;
transform_t to_transform(const pose_t__ &_p) noexcept;
transform_t__ to_transform(const transform_t &_t) noexcept;
transform_t__ to_transform(const transform_t &_t,
                           const stamp_t &_stamp,
                           const std::string &_frame,
                           const std::string &_child) noexcept;

} // namespace internal

namespace map {

using position_t = Eigen::Vector2f;
using coordinate_t = Eigen::Matrix<size_t, 2ul, 1ul>;

inline bool is_valid(const nav_msgs::OccupancyGrid &_map) noexcept {
  // data is only valid, if the product of height * width corresponds to
  // allocated vector size
  return _map.info.height * _map.info.width == _map.data.size();
}

struct map_data {

  explicit map_data(const nav_msgs::MapMetaData &m) noexcept;
  map_data(const coordinate_t &_size,
           float _resolution,
           const position_t &_origin) noexcept;

  float resolution;
  coordinate_t size;
  position_t origin;
};

struct converter {

  explicit converter(const map_data &p) noexcept;
  explicit converter(const nav_msgs::MapMetaData &m) noexcept;

  index_t to_index(const position_t &p) const;
  index_t to_index(const coordinate_t &c) const;
  coordinate_t to_coordinate(const position_t &p) const;
  coordinate_t to_coordinate(index_t i) const;
  position_t to_position(index_t _i) const;
  position_t to_position(const coordinate_t& _c) const;

  inline bool valid_position(const position_t &_p) const noexcept {
    const position_t relative = _p - p_.origin;
    if(relative.minCoeff() < 0){
      return false;
    }
    const coordinate_t out = (relative / p_.resolution).cast<size_t>();
    return valid_coordinate(out);
  }

  inline bool valid_coordinate(const coordinate_t& _c) const noexcept {
    return (_c.array() < p_.size.array()).prod() == 1;
  }

  inline bool valid(index_t _i) const noexcept {
    return _i < p_.size.prod();
  }

private:
  const map_data p_;
};

struct knn_t{
  using data_t = std::unordered_map<index_t, std::vector<position_t>>;

  explicit knn_t(const nav_msgs::MapMetaData& _grid, size_t _k) noexcept;
  explicit knn_t(const map_data& _parameter, size_t _k) noexcept;

  data_t knn_;
  size_t k_;
  const converter converter_;
};

using knn_ptr_t  = std::unique_ptr<knn_t>;
using map_value__ = signed char;

constexpr map_value__ occupied = 50;

inline bool is_occupied(map_value__ _val) noexcept {
  return _val >= occupied;
}

struct knn_parameter {

  static constexpr size_t k_max = 10;
  static constexpr size_t k_min = 1;
  static constexpr size_t k_def = 5;

  static constexpr size_t radius_max = 10;
  static constexpr size_t radius_min = 0;
  static constexpr size_t radius_def = 5;

  explicit knn_parameter(size_t _k = k_def, size_t _radius = radius_def) noexcept;
  explicit knn_parameter(ros::NodeHandle& _nh);

  size_t k_;
  size_t radius_;

  friend std::ostream &operator<<(std::ostream &_os,
                                  const knn_parameter &_p) noexcept;
};

struct is_reachable {

  explicit is_reachable(const nav_msgs::OccupancyGrid &_grid) noexcept;
  bool operator()(index_t _center) noexcept;

private:
  const nav_msgs::OccupancyGrid &grid_;
  const converter converter_;
};

knn_ptr_t interpret(const nav_msgs::OccupancyGrid &grid,
                    const knn_parameter &_p) noexcept;

struct interface {

  explicit interface(ros::NodeHandle &_nh);
  bool move_data(knn_ptr_t& _knn) noexcept;

private:
  void callback(const nav_msgs::OccupancyGridConstPtr &_msg) noexcept;

  knn_parameter p_;
  knn_ptr_t  knn_;
  ros::Subscriber sub_;
};

} // namespace map

namespace scan {

using scan_t = Eigen::Matrix<float, 2, Eigen::Dynamic, Eigen::RowMajor>;
using scan_ptr_t = std::unique_ptr<scan_t>;

using scan_view_t = Eigen::Map<const Eigen::Matrix<float, 1, Eigen::Dynamic>>;

struct parameter {

  explicit parameter(const sensor_msgs::LaserScan &_scan) noexcept;

  bool operator==(const parameter &_other) const noexcept;
  bool operator!=(const parameter &_other) const noexcept;

  const size_t size;
  const float increment, angle_min, angle_max;
};

using parameter_ptr_t = std::unique_ptr<parameter>;

scan_ptr_t cache(const parameter &_p) noexcept;

struct controller_base_ {
  virtual ~controller_base_() = default;
  virtual void update(const scan_t &_scan,
                      const stamp_t &_scan_stamp,
                      const std::string &_scan_frame) {};
};

struct interface_ {

  explicit interface_(controller_base_* _controller = nullptr);
  virtual ~interface_() = default;

protected:
  void callback(const sensor_msgs::LaserScanConstPtr &_msg);

  controller_base_* controller_;
  parameter_ptr_t p_;
  scan_ptr_t cache_;
};

struct interface : public interface_ {
  explicit interface(ros::NodeHandle &_nh, controller_base_* _controller = nullptr);

private:
  ros::Subscriber sub_;
};

} // namespace scan

namespace odom {

struct interface_tf {

  interface_tf();
  transform_t get(const std::string& _from, const std::string& _to);
  void set(const transform_t &_tf,
           const stamp_t &_stamp,
           const std::string &_from,
           const std::string &_to) noexcept;

private:
  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener tf_listener_;
  tf2_ros::TransformBroadcaster tf_broadcaster_;
};

struct interface_rviz {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  static constexpr char topic[] = "/initialpose";

  // todo pass ref to map_frame_ instead of node handle
  interface_rviz(ros::NodeHandle &_nh, transform_t &_map_to_base) noexcept;

private:
  void callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& _msg) noexcept;
  transform_t& map_to_base_;
  std::string map_frame_;
  ros::Subscriber sub_;
};

struct frame_handler {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  explicit frame_handler(ros::NodeHandle& _nh);

  transform_t get_map_to_base() noexcept ;
  transform_t get_map_to_odom() const noexcept;
  transform_t get_map_to_odom(const transform_t& _map_to_base) noexcept ;
  transform_t get_base_to_sensor(const std::string& _scan_frame) noexcept;

  void set_map_to_odom(const transform_t& _tf, const stamp_t& _stamp) noexcept;

private:
  void handle_map_to_base();
  void handle_base_to_sensor(const std::string& _scan_frame);

  transform_t map_to_base_, odom_to_base_, base_to_sensor_;
  interface_tf tf_;
  interface_rviz rviz_;

  std::string map_frame_;
  std::string base_frame_;
  std::string odom_frame_;
};

} // namespace odom

namespace icp {

struct matches{

  matches(const scan::scan_t& _sensor, const scan::scan_t& _map) noexcept;
  matches(const scan::scan_t& _scan, const map::knn_t& _knn) noexcept;

  scan::scan_t sensor_;
  scan::scan_t map_;
};

using weight_t = Eigen::VectorXf;
weight_t get_weights(const matches& _data);

transform_t point_to_map(matches&& _data, weight_t&& _w);

struct conversion_checker{

  static constexpr float t_norm = 0.01f;
  static constexpr float r_norm = 0.01f;

  explicit conversion_checker(float _t_norm = t_norm,
                              float _r_norm = r_norm) noexcept;
  explicit conversion_checker(ros::NodeHandle &_nh);

  bool operator()(const transform_t &_in) noexcept;

private:
  float t_norm_, r_norm_;
};

struct sampler {

  static constexpr size_t stride_def = 3;
  static constexpr size_t stride_min = 1;
  static constexpr size_t stride_max = 100;

  explicit sampler(size_t _stride = stride_def) noexcept;
  explicit sampler(ros::NodeHandle &_nh);

  scan::scan_t operator()(const scan::scan_t &_in) noexcept;

private:
  size_t stride_;

  std::random_device rd_;
  std::mt19937 gen_;

  using distribution_t = std::uniform_int_distribution<size_t>;
  distribution_t dist_;
};

struct scan_matcher {

  static constexpr size_t max_iter = 100;
  static constexpr size_t min_iter = 1;

  explicit scan_matcher(ros::NodeHandle& _nh);
  transform_t operator()(const scan::scan_t& _scan);

private:
  conversion_checker is_converged_;
  sampler sample_;
  map::knn_ptr_t knn_;
  map::interface map_;
  size_t iter_;
};

} // namespace icp

struct controller : public scan::controller_base_ {

  explicit controller(ros::NodeHandle& _nh);
  void update(const scan::scan_t &_scan,
              const stamp_t &_stamp,
              const std::string &_scan_frame) override;

private:
  icp::scan_matcher icp_;
  scan::interface scan_interface_;
  odom::frame_handler frame_handler_;
};

} // namespace i_see_pee

#endif //I_SEE_PEE_I_SEE_PEE_HPP


#include <iostream>
#include <Eigen/Dense>
#include <random>
#include <vector>
#include <iomanip>
#include <fstream>
#include <bitset>
#include <string>
#include <map>

#ifndef QST_TESTTOMOGRPHY_HPP
#define QST_TESTTOMOGRPHY_HPP

namespace qst{

//Quantum State Tomography class
template<class NNState,class Observer> class Test{

    NNState & NNstate_;                   // Neural network representation of the state
    Observer &obs_;

    int N_;
    int npar_;                          // Number of variational parameters
    int nparLambda_;                    // Number of amplitude variational parameters
    int nparMu_;
    std::string basis_;
    Eigen::VectorXd grad_;         // Gradient
    Eigen::VectorXcd rotated_grad_;
    std::mt19937 rgen_;                 // Random number generator
    std::map<std::string,Eigen::MatrixXcd> U_;
    Eigen::MatrixXd basis_states_;
    Eigen::VectorXcd target_psi_;
    std::vector<Eigen::VectorXcd> rotated_wf_;
    std::vector<std::vector<std::string> > basisSet_;

public:

    Test(NNState & NNstate,Observer &obs,Parameters &par):NNstate_(NNstate),obs_(obs),N_(NNstate.N()){

        npar_=NNstate_.Npar();
        nparLambda_ = NNstate_.NparLambda();
        nparMu_ = npar_ - nparLambda_;
        grad_.resize(npar_);
        rotated_grad_.resize(npar_);
        basis_ = par.basis_;
        basis_states_.resize(1<<N_,N_);
        std::bitset<10> bit;
        // Create the basis of the Hilbert space
        for(int i=0;i<1<<N_;i++){
            bit = i;
            for(int j=0;j<N_;j++){
                basis_states_(i,j) = bit[N_-j-1];
            }
        }

    }

    // Test the derivatives of the KL divergence
    void DerKL(double eps=1.0e-4){
        auto pars = NNstate_.GetParameters();
        obs_.ExactPartitionFunction();
        Eigen::VectorXcd derKL(npar_);
        Eigen::VectorXd ders(npar_);
        ders.setZero(npar_);

        //-- ALGORITHMIC DERIVATIVES --//
        //Standard Basis
        for(int j=0;j<1<<N_;j++){
            //Positive phase - Lambda gradient in reference basis
            ders.head(nparLambda_) +=  norm(target_psi_(j))*NNstate_.LambdaGrad(basis_states_.row(j));
            //Negative phase - Lambda gradient in reference basis
            ders.head(nparLambda_) -= NNstate_.LambdaGrad(basis_states_.row(j))*norm(NNstate_.psi(basis_states_.row(j)))/obs_.Z_;
            //std::cout << (NNstate_.LambdaGrad(basis_states_.row(j)).head(10)).transpose() << std::endl<<std::endl;
        }
        if (basis_.compare("std")!=0){
            //Rotated Basis
            for(int b=1;b<basisSet_.size();b++){
                for(int j=0;j<1<<N_;j++){
                    NNstate_.rotatedGrad(basisSet_[b],basis_states_.row(j),U_,derKL);
                    //Positive phase - Lambda gradient in basis b
                    ders.head(nparLambda_) += norm(rotated_wf_[b-1](j))*derKL.head(nparLambda_).real();
                    //Positive phase - Mu gradient in basis b
                    ders.tail(nparMu_) -= norm(rotated_wf_[b-1](j))*derKL.tail(nparMu_).imag();
                    //Negative phase - Lambda gradient in basis b (identical to the reference basis
                    ders.head(nparLambda_) -= NNstate_.LambdaGrad(basis_states_.row(j))*norm(NNstate_.psi(basis_states_.row(j)))/obs_.Z_;
                }
            }
        }
        //-- NUMERICAL DERIVATIVES --//
        for(int p=0;p<npar_;p++){
            pars(p)+=eps;
            NNstate_.SetParameters(pars);
            double valp=0.0;
            obs_.ExactPartitionFunction();
            obs_.ExactKL();
            valp = obs_.KL_;
            pars(p)-=2*eps;
            NNstate_.SetParameters(pars);
            double valm=0.0;
            obs_.ExactPartitionFunction();
            obs_.ExactKL();
            valm = obs_.KL_;
            pars(p)+=eps;
            double numder=(-valm+valp)/(eps*2);
            std::cout<<"Derivative wrt par "<<p<<". Grad =: "<<ders(p)<<" Numerical = : "<<numder<<std::endl;
        }
    }

    // Test the derivatives of the KL divergence
    void DerNLL(Eigen::MatrixXd &data,double eps=1.0e-4){
        auto pars = NNstate_.GetParameters();
        obs_.ExactPartitionFunction();
        Eigen::VectorXcd derKL(npar_);
        Eigen::VectorXd ders(npar_);
        ders.setZero(npar_);

        //-- ALGORITHMIC DERIVATIVES --//
        //Standard Basis
        std::cout << data.rows() <<std::endl;
        for(int j=0;j<data.rows();j++){
            //Positive phase - Lambda gradient in reference basis
            ders.head(nparLambda_) +=  NNstate_.LambdaGrad(data.row(j))/float(data.rows());
        }
        //for(int j=0;j<1<<N_;j++){
        //    ders.head(nparLambda_) -= NNstate_.LambdaGrad(basis_states_.row(j))*norm(NNstate_.psi(basis_states_.row(j)))/obs_.Z_;
        //    //std::cout << (NNstate_.LambdaGrad(basis_states_.row(j)).head(10)).transpose() << std::endl<<std::endl;
        //}
        //NNstate_.SetVisibleLayer(data);
        NNstate_.Sample(100);
        std::cout << NNstate_.VisibleStateRow(0) << std::endl;
        for(int k=0;k<NNstate_.Nchains();k++){
            ders.head(nparLambda_) -= NNstate_.LambdaGrad(NNstate_.VisibleStateRow(k))/double(NNstate_.Nchains());
        }
        //if (basis_.compare("std")!=0){
        //    //Rotated Basis
        //    for(int b=1;b<basisSet_.size();b++){
        //        for(int j=0;j<1<<N_;j++){
        //            NNstate_.rotatedGrad(basisSet_[b],basis_states_.row(j),U_,derKL);
        //            //Positive phase - Lambda gradient in basis b
        //            ders.head(nparLambda_) += norm(rotated_wf_[b-1](j))*derKL.head(nparLambda_).real();
        //            //Positive phase - Mu gradient in basis b
        //            ders.tail(nparMu_) -= norm(rotated_wf_[b-1](j))*derKL.tail(nparMu_).imag();
        //            //Negative phase - Lambda gradient in basis b (identical to the reference basis
        //            ders.head(nparLambda_) -= NNstate_.LambdaGrad(basis_states_.row(j))*norm(NNstate_.psi(basis_states_.row(j)))/obs_.Z_;
        //        }
        //    }
        //}
        //-- NUMERICAL DERIVATIVES --//
        for(int p=0;p<npar_;p++){
            pars(p)+=eps;
            NNstate_.SetParameters(pars);
            double valp=0.0;
            obs_.ExactPartitionFunction();
            obs_.NLL(data);
            valp = obs_.NLL_;
            pars(p)-=2*eps;
            NNstate_.SetParameters(pars);
            double valm=0.0;
            obs_.ExactPartitionFunction();
            obs_.NLL(data);
            valm = obs_.NLL_;
            pars(p)+=eps;
            double numder=(-valm+valp)/(eps*2);
            std::cout<<"Derivative wrt par "<<p<<". Grad =: "<<ders(p)<<" Numerical = : "<<numder<<std::endl;
        }
    }
    //Set the value of the target wavefunction
    void setBasisRotations(std::map<std::string,Eigen::MatrixXcd> & U){
        U_ = U;
    }
    void setBasis(std::vector<std::vector<std::string> > basis) {
        basisSet_ = basis;
    }
    //Set the value of the target wavefunction
    void setWavefunction(Eigen::VectorXcd & psi){
        target_psi_.resize(1<<N_);
        for(int i=0;i<1<<N_;i++){
            target_psi_(i) = psi(i);
        }
    }
    void setRotatedWavefunctions(std::vector<Eigen::VectorXcd> & psi){
        for(int b=0;b<psi.size();b++){
            rotated_wf_.push_back(psi[b]);
        }
    }
};
}

#endif


#include <leeloo/interval.h>
#include <leeloo/list_intervals.h>
#include <leeloo/uni.h>
#include <leeloo/random.h>

#include <boost/random.hpp>
#include <vli/integer.hpp>

typedef vli::integer<128> u128;
typedef leeloo::interval<u128> u128_interval;
typedef leeloo::list_intervals<u128_interval> u128_list_intervals;

int main()
{
	boost::random::mt19937 mt_rand(time(NULL));
	u128_list_intervals li;
	li.add(u128(10), u128(20));
	li.aggregate();
	li.random_sets<leeloo::uni>(16, [](u128 const* buf, size_t const n) { },
			leeloo::random_engine<uint32_t>(mt_rand));

	return 0;
}


#include <boost/numeric/conversion/cast.hpp>
#include "renderer.h"
#include "memory_model.h"
#include "memory_mesh.h"

namespace graphic {

MemoryMesh::MemoryMesh(MemoryModel *m):
	Mesh(m)
{
}

MemoryMesh::~MemoryMesh()
{
}

math::aabb<3> const &MemoryMesh::get_bounds() const
{
	throw std::runtime_error("not supported");
}

MeshPtr MemoryMesh::shallow_copy() const
{
	return MeshPtr(new MemoryMesh(*this));
}

void MemoryMesh::set_vertices(GenericVertex *vertices, GenericVertex::format_type format, size_t count)
{
	vertices_.resize(count);
	weights_.resize(count);

	{
		GenericVertex *vertex = vertices;

		if (initialized_vertex_components_ == 0 || format == vertex_type::FORMAT)
		{
			for (std::vector<vertex_type>::iterator it = vertices_.begin(); it != vertices_.end(); it++)
			{
				*it = *vertex++;
			}
		}
		else
		{
			for (std::vector<vertex_type>::iterator it = vertices_.begin(); it != vertices_.end(); it++)
			{
				it->assign(*vertex++, format);
			}
		}
	}

	{
		GenericVertex *vertex = vertices;

		for (std::vector<JointWeights>::iterator it = weights_.begin(); it != weights_.end(); it++)
		{
			if (format & GenericVertex::FORMAT_JOINT_INDICES) it->joints = vertex->joints;
			if (format & GenericVertex::FORMAT_JOINT_WEIGHTS) it->weights = vertex->weights;

			++vertex;
		}
	}

	initialized_vertex_components_ |= format;
}

void MemoryMesh::set_indices(unsigned *indices, size_t count)
{
	indices_.resize(count);

	for (size_t i = 0; i < count; ++i)
	{
		indices_[i] = index_type(*indices++);
	}
}

void MemoryMesh::set_always_visible(bool /*flag*/)
{
	// frustum culling is not supported for memory mesh
}

void MemoryMesh::transform_vertices(Joint const *joint_ptr, std::vector<vertex_type> &result) const
{
    std::vector<math::matrix<4,4> > joint_matrix_array;
    joint_matrix_array.reserve(joint_index_transform.size());

    for (joint_index_transform_type::const_iterator it = joint_index_transform.begin();
         it != joint_index_transform.end(); it++)
    {
		math::matrix<4,4> t = bind_shape_matrix *
			inverted_bind_matrices[*it] *
			get_model()->joints[*it]->matrix;

        joint_matrix_array.push_back(t);
    }

	result = vertices_;

	for (size_t i = 0; i < vertices_.size(); i++)
	{
		math::vec<3> pos(0, 0, 0);

		math::vec<8, unsigned> const &indices = weights_[i].joints;
		math::vec<8> const &weights = weights_[i].weights;

		for (size_t j = 0; j < 8; j++)
		{
			if (weights.i[j] > math::EPSILON)
			{
				pos += (vertices_[i].position * joint_matrix_array.at(indices.i[j])) * weights.i[j];
			}
		}

		result[i].position = pos;
	}
}

// Results of rendering via MemoryMesh and AnimatedMesh may differ if model have joint weights
// and no animation: AnimatedMesh always computes positions of vertices using weights but MemoryMesh
// uses weights only if model have animation. You can easily check if this is the cause just replacing
// AnimatedMesh with StaticMesh and checking that rendering result is matching the one of MemoryMesh.
void MemoryMesh::draw_static(Joint *joint_ptr, CameraPtr const &camera,
	math::matrix<4,4> const &world_matrix) const
{
	// TODO
/*
	RendererPtr const renderer_ptr = get_model()->get_renderer();
    ShadersLibrary *shaders_library_ptr = renderer_ptr->get_shaders_library();
	IDirect3DDevice9 *device_ptr = renderer_ptr->get_device();

    device_ptr->SetVertexDeclaration(renderer_ptr->get_vertex_declarations()->get<vertex_type>());

	for (triangles_list_type::const_iterator it = triangles_list.begin();
		it != triangles_list.end(); it++)
	{
		it->material_ptr->select();

		device_ptr->SetTransform(D3DTS_WORLD, (D3DMATRIX const *) joint_ptr->matrix.ij);

		device_ptr->DrawIndexedPrimitiveUP(D3DPT_TRIANGLELIST, it->minimal_vertex_index, it->vertices_count,
			it->triangles_count, &indices_[it->first_index_offset], INDEX_FORMAT_U16, &vertices_[0], sizeof(vertex_type));
	}
*/
}

void MemoryMesh::draw_animated(Joint *joint_ptr, CameraPtr const &camera,
	math::matrix<4,4> const &world_matrix) const
{
	// TODO
/*
	RendererPtr const renderer_ptr = get_model()->get_renderer();
    ShadersLibrary *shaders_library_ptr = renderer_ptr->get_shaders_library();
	IDirect3DDevice9 *device_ptr = renderer_ptr->get_device();

	std::vector<vertex_type> transformed_vertices;
	transform_vertices(joint_ptr, transformed_vertices);

	device_ptr->SetVertexDeclaration(renderer_ptr->get_vertex_declarations()->get<vertex_type>());

	for (triangles_list_type::const_iterator it = triangles_list.begin();
		it != triangles_list.end(); it++)
	{
		it->material_ptr->select();

		device_ptr->SetTransform(D3DTS_WORLD, (D3DMATRIX const *) joint_ptr->matrix.ij);

		device_ptr->DrawIndexedPrimitiveUP(D3DPT_TRIANGLELIST, it->minimal_vertex_index, it->vertices_count,
			it->triangles_count, &indices_[it->first_index_offset], INDEX_FORMAT_U16, &transformed_vertices[0], sizeof(vertex_type));
	}
*/
}

}


#include <iostream>
#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>

#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

int main()
{
  typedef CGAL::Point_2<Kernel> Point;
  typedef std::vector<Point> Linestring;
  typedef CGAL::Polygon_with_holes_2<Kernel> Cgal_polygon;
  typedef std::vector<Point> MultiPoint;
  typedef std::vector<Linestring> MultiLinestring;
  typedef std::vector<Cgal_polygon> MultiPolygon;

  Point p;
  {
    std::ifstream in("data/point.wkt");
    CGAL::read_point_WKT(in, p);
    in.close();
    CGAL_assertion(p == Point(2,3));
  }
  {
    std::ifstream in("data/linestring.wkt");
    Linestring ls;
    CGAL::read_linestring_WKT(in, ls);
    in.close();
    CGAL_assertion(ls.size() == 3);
  }
  {
    Cgal_polygon poly;
    std::ifstream in("data/polygon.wkt");
    CGAL::read_polygon_WKT(in, poly);
    in.close();
    CGAL_assertion(poly.outer_boundary().size() == 3);
  }
  {
    MultiPoint pees;
    std::ifstream in("data/multipoint.wkt");
    CGAL::read_multi_point_WKT(in, pees);
    in.close();
    CGAL_assertion(pees.size() == 4);
  }
  {
    std::ifstream in("data/multilinestring.wkt");
    MultiLinestring mls;
    CGAL::read_multi_linestring_WKT(in, mls);
    in.close();
    CGAL_assertion(mls.size() == 2);
  }
  {
    MultiPolygon polies;
    std::ifstream in("data/multipolygon.wkt");
    CGAL::read_multi_polygon_WKT(in, polies);
    in.close();
    CGAL_assertion(polies.size() == 2);
  }
  std::cout<<"WKT reading test passed."<<std::endl;
  return 0;
}
#else
int main()
{
  return 0;
}
#endif


#include <boost/mpl/assert.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/simd/pack.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::is_same;
using nt2::meta::as_real;

using nt2::int32_t;
using nt2::uint32_t;
using nt2::uint64_t;
using nt2::simd::pack;

int main()
{
  BOOST_MPL_ASSERT(( is_same<float,       as_real<int32_t>::type>         ));
  BOOST_MPL_ASSERT(( is_same<float,       as_real<uint32_t>::type>        ));
  BOOST_MPL_ASSERT(( is_same<double,      as_real<uint64_t>::type>        ));
  BOOST_MPL_ASSERT(( is_same<pack<float>, as_real< pack<int32_t> >::type> ));
}


#include <boost/hana/maximum.hpp>


#include "stdafx.h"
#include "CppUnitTest.h"

#include <vector>
#include <algorithm>
#include <boost\timer.hpp>
#include <CppUnitTestLogger.h>
#include "UnitTestAux.h"
#include "../BVP/Utils/AuxUtils.h"
#include "../BVP/Utils/SolutionUtils.h"
#include <string>

using namespace Microsoft::VisualStudio::CppUnitTestFramework;
namespace GeneralTest
{
TEST_CLASS(simplification_test)
{
public:
	template <bool Deriv>
	void Run_simplification_test()
	{
		const auto solution = auxutils::ReadFromFile<InitCondition<double>>("TestData\\solution_double_precision.txt");
		const auto length_init = solutionutils::length<Deriv>(solution);

		boost::timer t;

		double percentage_of_points_to_keep = 0.01;
		const auto solution_simplified = solutionutils::simplify_polyline<Deriv>(solution, percentage_of_points_to_keep);

		Logger::WriteMessage((std::string("Simplification time : ") + auxutils::ToString(t.elapsed())+"\n").c_str());

		const auto length_simplified = solutionutils::length<Deriv>(solution_simplified);

		auto diff = std::abs(length_init - length_simplified);

		Logger::WriteMessage((std::string("Length before simplification : ") + auxutils::ToString(length_init)+"\n").c_str());
		Logger::WriteMessage((std::string("Length after simplification : ") + auxutils::ToString(length_simplified) + "\n").c_str());
		Logger::WriteMessage((std::string("Length difference : ") + auxutils::ToString(diff)+"\n").c_str());

		Assert::IsTrue(diff < 2e-7, L"Too big difference in th elength before and after simplification");
		Assert::IsTrue(solution_simplified.size()/solution.size() <= percentage_of_points_to_keep, L"Unexpectedly many points have been left");

		//auxutils::SaveToFile(solution_simplified, "E:\\Research\\Derivative_simplified_double_precision.txt");
		//auxutils::SaveFunction<Deriv>("x", "u", "E:\\Research\\derivative_to_plot.txt", solution_simplified);
	}

	TEST_METHOD(Function_simplification_test)
	{
		Run_simplification_test<false>();
	}

	TEST_METHOD(Derivative_simplification_test)
	{
		Run_simplification_test<true>();
	}
};
}


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

/// @file
/// Test if the specific heat is computed correctly.

#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <bulk_properties.hpp>

TEST(cv_case, cv_test) {
    boost::mpi::environment env;

    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto si_dir = basedir / boost::filesystem::path("Si");
    auto poscar_path = si_dir / boost::filesystem::path("POSCAR");
    auto ifc_path = si_dir / boost::filesystem::path("FORCE_CONSTANTS");

    auto poscar = alma::load_POSCAR(poscar_path.string().c_str());
    auto syms = alma::Symmetry_operations(*poscar);
    auto force_constants =
        alma::load_FORCE_CONSTANTS(ifc_path.string().c_str(), *poscar, 5, 5, 5);

    auto grid = std::make_unique<alma::Gamma_grid>(
        *poscar, syms, *force_constants, 12, 12, 12);

    grid->enforce_asr();

    auto cv = alma::calc_cv(*poscar, *grid, 300.);
    EXPECT_TRUE(alma::almost_equal(1e27 * cv, 1.625834959e6));
}


//==================================================================================================
/**
  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_COMMON_SIMD_FUNCTION_FREXP_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_FREXP_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/ifrexp.hpp>
#include <boost/simd/function/tofloat.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator()(A0 const& a0) const
    {
      auto r = ifrexp(a0);
      return {r.first, tofloat(r.second)};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pedantic_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator()(bs::pedantic_tag const&, A0 const& a0) const
    {
      auto r = pedantic_(ifrexp)(a0);
      return {r.first, tofloat(r.second)};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( frexp_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::raw_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE std::pair<A0,A0> operator()(bs::raw_tag const&, A0 const& a0) const
    {
      auto r = raw_(ifrexp)(a0);
      return {r.first, tofloat(r.second)};
    }
  };
} } }

#endif


#define BOOST_TEST_MODULE traxels_test

#include <iostream>
#include <sstream>
#include <string>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/tuple/tuple.hpp>

#include "pgmlink/traxels.h"
#include "pgmlink/field_of_view.h"

using namespace pgmlink;
using namespace std;
using namespace boost;

BOOST_AUTO_TEST_CASE( Locator_serialize )
{
  ComLocator* cl = new ComLocator;
  cl->x_scale = 2.;
  cl->y_scale = 3.;
  cl->z_scale = 4.;

  IntmaxposLocator* il = new IntmaxposLocator;
  il->x_scale = 20.;
  il->y_scale = 30.;
  il->z_scale = 40.;

  FeatureMap com_m;
  FeatureMap intmaxpos_m;
  feature_array f;
  f.push_back(4);
  f.push_back(5);
  f.push_back(6);
  com_m["com"] = f;
  intmaxpos_m["intmaxpos"] = f;

  // save to string
  string s;
  {
    stringstream ss;
    boost::archive::text_oarchive oa(ss);
    oa << cl << il;
    s = ss.str();
  }

  // load from string and compare
  ComLocator* loaded_com;
  IntmaxposLocator* loaded_int;
  {
    stringstream ss(s);
    boost::archive::text_iarchive ia(ss);
    ia >> loaded_com >> loaded_int;
  }

  BOOST_CHECK( loaded_com != cl );
  BOOST_CHECK( loaded_int != il );

  BOOST_REQUIRE(loaded_com->is_applicable(com_m));
  BOOST_REQUIRE(!loaded_com->is_applicable(intmaxpos_m));
  BOOST_REQUIRE(!loaded_int->is_applicable(com_m));
  BOOST_REQUIRE(loaded_int->is_applicable(intmaxpos_m));

  BOOST_CHECK_EQUAL(loaded_com->x_scale, cl->x_scale);
  BOOST_CHECK_EQUAL(loaded_com->y_scale, cl->y_scale);
  BOOST_CHECK_EQUAL(loaded_com->z_scale, cl->z_scale);
  BOOST_CHECK_EQUAL(loaded_int->x_scale, il->x_scale);
  BOOST_CHECK_EQUAL(loaded_int->y_scale, il->y_scale);
  BOOST_CHECK_EQUAL(loaded_int->z_scale, il->z_scale);
}

BOOST_AUTO_TEST_CASE( Traxel_assignment_op )
{
  Traxel t1,t2;
  t1.Id = 1;
  t2.Id = 2;
  t2 = t1;
  BOOST_CHECK_EQUAL(t1.Id, t2.Id);
}

BOOST_AUTO_TEST_CASE( Traxel_XYZ )
{
  Traxel t;
  feature_array com(3);
  com[0] = 34; com[1] = 45; com[2] = 12.3;
  t.features["com"] = com;
  BOOST_CHECK_EQUAL(t.X(), com[0]);
  BOOST_CHECK_EQUAL(t.Y(), com[1]);
  BOOST_CHECK_EQUAL(t.Z(), com[2]);
}

BOOST_AUTO_TEST_CASE( Traxel_resolution_scaling )
{
  ComLocator* locator = new ComLocator();
  locator->x_scale = 0.5;
  locator->z_scale = 12.3;
  Traxel t;
  t.set_locator(locator);
  feature_array com(3);
  com[0] = 34; com[1] = 45; com[2] = 12.3;
  t.features["com"] = com;
  BOOST_CHECK_EQUAL(t.X(), 0.5 * com[0]);
  BOOST_CHECK_EQUAL(t.Y(), com[1]);
  BOOST_CHECK_EQUAL(t.Z(), 12.3 * com[2]);
}

BOOST_AUTO_TEST_CASE( Traxel_distance_to )
{
    // prepare mock objects
    Traxel from, to;
    feature_array com1(3);
    feature_array com2(3);

    com1[0] = 1;
    com1[1] = 2;
    com1[2] = 0;
    from.features["com"] = com1;

    com2[0] = 2;
    com2[1] = 3;
    com2[2] = 7;
    to.features["com"] = com2;

    double distance = from.distance_to(to);
    BOOST_CHECK_CLOSE( distance, 7.1414284285, 0.01);
 }

BOOST_AUTO_TEST_CASE( Traxel_angle )
{
    // prepare mock objects
    Traxel vertex, leg1, leg2;
    feature_array com1(3);
    feature_array com2(3);
    feature_array com3(3);

    com1[0] = 1;
    com1[1] = 2;
    com1[2] = 0;
    vertex.features["com"] = com1;

    com2[0] = 2;
    com2[1] = 3;
    com2[2] = 7;
    leg1.features["com"] = com2;

    com3[0] = 1;
    com3[1] = 0.5;
    com3[2] = 1.5;
    leg2.features["com"] = com3;

    double angle = vertex.angle(leg1, leg2);
    BOOST_CHECK_CLOSE( angle, 0.93466, 0.01);
    angle = vertex.angle(leg2, leg1);
    BOOST_CHECK_CLOSE( angle, 0.93466, 0.01);

    // and another one
    com1[0] = 1;
    com1[1] = 2;
    com1[2] = 0;
    vertex.features["com"] = com1;

    com2[0] = 2;
    com2[1] = 3;
    com2[2] = 0;
    leg1.features["com"] = com2;

    com3[0] = 3;
    com3[1] = 1;
    com3[2] = 0;
    leg2.features["com"] = com3;

    angle = vertex.angle(leg1, leg2);
    BOOST_CHECK_CLOSE( angle, 1.2490, 0.01);
    angle = vertex.angle(leg2, leg1);
    BOOST_CHECK_CLOSE( angle, 1.2490, 0.01);

    // check angle > 90°
    com1[0] = 1;
    com1[1] = 0;
    com1[2] = 0;
    vertex.features["com"] = com1;

    com2[0] = 2;
    com2[1] = 0;
    com2[2] = 0;
    leg1.features["com"] = com2;

    com3[0] = 0;
    com3[1] = 0;
    com3[2] = 0;
    leg2.features["com"] = com3;

    angle = vertex.angle(leg1, leg2);
    BOOST_CHECK_CLOSE( angle, 3.1415, 0.01);
    angle = vertex.angle(leg2, leg1);
    BOOST_CHECK_CLOSE( angle, 3.1415, 0.01);

}

BOOST_AUTO_TEST_CASE( Traxel_serialize )
{
    Traxel t1;
    t1.Timestep = 2;
    t1.Id = 12;
    t1.set_locator( new IntmaxposLocator );
    feature_array intmaxpos;
    intmaxpos.push_back(4);
    intmaxpos.push_back(5);
    intmaxpos.push_back(6);
    t1.features["intmaxpos"] = intmaxpos;

    // save to string
    string s;
    {
      stringstream ss;
      boost::archive::text_oarchive oa(ss);
      oa << t1;
      s = ss.str();
    }

    // load from string and compare
    Traxel loaded;
    {
      stringstream ss(s);
      boost::archive::text_iarchive ia(ss);
      ia >> loaded;
    }
    BOOST_CHECK_EQUAL(loaded.Id, t1.Id);
    BOOST_CHECK_EQUAL(loaded.Timestep, t1.Timestep);
    BOOST_REQUIRE(loaded.features.count("intmaxpos")==1);
    BOOST_CHECK_EQUAL_COLLECTIONS(loaded.features["intmaxpos"].begin(),
				  loaded.features["intmaxpos"].end(),
				  intmaxpos.begin(),
				  intmaxpos.end());
    BOOST_CHECK( typeid(*loaded.locator()) == typeid(*t1.locator()) );
}

BOOST_AUTO_TEST_CASE( Traxel_strict_weak_ordering )
{
    Traxel t1, t2;
    t1.Timestep = 1;
    t2.Timestep = 2;
    t1.Id = 1;
    t2.Id = 2;
    BOOST_CHECK(t1 < t2);
    BOOST_CHECK(!(t2 < t1));

    t1.Timestep = 2;
    t2.Timestep = 1;
    t1.Id = 1;
    t2.Id = 2;
    BOOST_CHECK(t2 < t1);
    BOOST_CHECK(!(t1 < t2));

    t1.Timestep = 1;
    t2.Timestep = 1;
    t1.Id = 1;
    t2.Id = 1;
    BOOST_CHECK(!(t1 < t2 || t2 < t1));
}

BOOST_AUTO_TEST_CASE( global_fun_nested_vec_from )
{
  Traxel t1,t21,t22,t4;
  t1.Timestep = 1;
  t1.Id = 0;
  t21.Timestep = 2;
  t21.Id = 0;
  t22.Timestep = 2;
  t22.Id = 1;
  t4.Timestep = 4;
  TraxelStore t;
  t.get<by_timestep>().insert(t1);
  t.get<by_timestep>().insert(t21);
  t.get<by_timestep>().insert(t22);
  t.get<by_timestep>().insert(t4);

  vector<vector<Traxel> > nested_vec;
  nested_vec = nested_vec_from(t);
  BOOST_REQUIRE_EQUAL(nested_vec.size(), 4);
  BOOST_CHECK_EQUAL(nested_vec[0].size(), 1);
  BOOST_CHECK_EQUAL(nested_vec[1].size(), 2);
  BOOST_CHECK_EQUAL(nested_vec[2].size(), 0);
  BOOST_CHECK_EQUAL(nested_vec[3].size(), 1);
}

BOOST_AUTO_TEST_CASE( global_fun_filter_by_fov )
{
  FieldOfView fov( 1,1,1,1, 2,2,2,2 );
  Traxel outside, inside, on_the_border, in_space_out_time, out_space_in_time;
  feature_array com1(3), com2(3), com3(3);
  com1[0] = 0.5; com1[1] = 2.5; com1[2] = 1;
  com2[0] = 1.5; com2[1] = 1.5; com2[2] = 1.5;
  com3[0] = 2;   com3[1] = 2;   com3[2] = 2;

  outside.features["com"] = com1;
  outside.Timestep = 3;
  outside.Id = 0;

  inside.features["com"] = com2;
  inside.Timestep = 2;
  inside.Id = 1;

  on_the_border.features["com"] = com3;
  on_the_border.Timestep = 1;
  on_the_border.Id = 2;

  in_space_out_time.features["com"] = com2;
  in_space_out_time.Timestep = 0;
  in_space_out_time.Id = 3;

  out_space_in_time.features["com"] = com1;
  out_space_in_time.Timestep = 2;
  out_space_in_time.Id = 4;

  Traxel traxels[] = {outside, inside, on_the_border, in_space_out_time, out_space_in_time};
  TraxelStore ts, ts_out;
  add(ts, traxels, traxels + (sizeof(traxels) / sizeof(Traxel)));
  BOOST_CHECK_EQUAL(ts.size(), 5);
  BOOST_CHECK_EQUAL(ts_out.size(), 0);

  size_t n = filter_by_fov(ts, ts_out, fov);
  BOOST_CHECK_EQUAL(ts.size(), 5);
  BOOST_CHECK_EQUAL(n, 2);
  BOOST_CHECK_EQUAL(ts_out.size(), 2);
  BOOST_CHECK_EQUAL(ts_out.get<by_timeid>().count(tuple<int, unsigned int>(2,1)), 1);
  BOOST_CHECK_EQUAL(ts_out.get<by_timeid>().count(tuple<int, unsigned int>(1,2)), 1);
}
// EOF


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_ITERATOR_COUNTING_ITERATOR_HPP
#define BOOST_COMPUTE_ITERATOR_COUNTING_ITERATOR_HPP

#include <string>
#include <cstddef>
#include <iterator>

#include <boost/config.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include <boost/compute/detail/meta_kernel.hpp>
#include <boost/compute/type_traits/is_device_iterator.hpp>

namespace boost {
namespace compute {

// forward declaration for counting_iterator<T>
template<class T> class counting_iterator;

namespace detail {

// helper class which defines the iterator_facade super-class
// type for counting_iterator<T>
template<class T>
class counting_iterator_base
{
public:
    typedef ::boost::iterator_facade<
        ::boost::compute::counting_iterator<T>,
        T,
        ::std::random_access_iterator_tag
    > type;
};

template<class T, class IndexExpr>
struct counting_iterator_index_expr
{
    typedef T result_type;

    counting_iterator_index_expr(const T &init, const IndexExpr &expr)
        : m_init(init),
          m_expr(expr)
    {
    }

    const T &m_init;
    IndexExpr m_expr;
};

template<class T, class IndexExpr>
inline meta_kernel& operator<<(meta_kernel &kernel,
                               const counting_iterator_index_expr<T, IndexExpr> &expr)
{
    return kernel << '(' << expr.m_init << '+' << expr.m_expr << ')';
}

} // end detail namespace

/// \class counting_iterator
/// \brief The counting_iterator class implements a counting iterator.
///
/// A counting iterator returns an internal value (initialized with \p init)
/// which is incremented each time the iterator is incremented.
///
/// For example, this could be used to implement the iota() algorithm in terms
/// of the copy() algorithm by copying from a range of counting iterators:
///
/// \snippet test/test_counting_iterator.cpp iota_with_copy
///
/// \see make_counting_iterator()
template<class T>
class counting_iterator : public detail::counting_iterator_base<T>::type
{
public:
    typedef typename detail::counting_iterator_base<T>::type super_type;
    typedef typename super_type::reference reference;
    typedef typename super_type::difference_type difference_type;

    counting_iterator(const T &init)
        : m_init(init)
    {
    }

    counting_iterator(const counting_iterator<T> &other)
        : m_init(other.m_init)
    {
    }

    counting_iterator<T>& operator=(const counting_iterator<T> &other)
    {
        if(this != &other){
            m_init = other.m_init;
        }

        return *this;
    }

    ~counting_iterator()
    {
    }

    size_t get_index() const
    {
        return 0;
    }

    template<class Expr>
    detail::counting_iterator_index_expr<T, Expr>
    operator[](const Expr &expr) const
    {
        return detail::counting_iterator_index_expr<T, Expr>(m_init, expr);
    }

private:
    friend class ::boost::iterator_core_access;

    reference dereference() const
    {
        return m_init;
    }

    bool equal(const counting_iterator<T> &other) const
    {
        return m_init == other.m_init;
    }

    void increment()
    {
        m_init++;
    }

    void decrement()
    {
        m_init--;
    }

    void advance(difference_type n)
    {
        m_init += static_cast<T>(n);
    }

    difference_type distance_to(const counting_iterator<T> &other) const
    {
        return difference_type(other.m_init) - difference_type(m_init);
    }

private:
    T m_init;
};

/// Returns a new counting_iterator starting at \p init.
///
/// \param init the initial value
///
/// \return a counting_iterator with \p init.
///
/// For example, to create a counting iterator which returns unsigned integers
/// and increments from one:
/// \code
/// auto iter = make_counting_iterator<uint_>(1);
/// \endcode
template<class T>
inline counting_iterator<T> make_counting_iterator(const T &init)
{
    return counting_iterator<T>(init);
}

/// \internal_ (is_device_iterator specialization for counting_iterator)
template<class T>
struct is_device_iterator<counting_iterator<T> > : boost::true_type {};

} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_ITERATOR_COUNTING_ITERATOR_HPP


#include <memory>

#include <thread>

#include <viz3d/ui.h>
#include <viz3d/vtk_window.h>
#include <misc/cpp/imgui_stdlib.h>

#include <vtkConeSource.h>
#include <vtkNamedColors.h>
#include <vtkNew.h>
#include <vtkPolyDataMapper.h>
#include <vtkProperty.h>
#include <vtkStructuredPoints.h>
#include <vtkPointData.h>
#include <vtkFloatArray.h>
#include <vtkCellData.h>

#include <Eigen/Dense>

// Define a Custom viz3d::ImGuiWindow to add custom ImGUI Components
struct TestWindow : viz3d::ImGuiWindow {

    TestWindow(std::string &&winname) : viz3d::ImGuiWindow(std::move(winname)) {}

    void DrawImGUIContent() override {
        ImGui::Text("This is a Custom Defined ImGui Window !");
        ImGui::InputText("Write a string", &test_string);
        ImGui::Text(test_string.c_str());
    }

    std::string test_string;
};

// A Point Cloud Actor (fills the content of a PolyData), with a scalar field
auto GetPointCloudActor() {
    auto poly_data = vtkSmartPointer<vtkPolyData>::New();
    int num_points_per_primitive = 10000;
    int num_primitives = 3;
    int num_points = num_primitives * num_points_per_primitive;

    // Setup an array of Points
    vtkNew<vtkPoints> points;
    points->SetDataTypeToFloat();
    points->Allocate(num_points);
    points->SetNumberOfPoints(num_points);
    points->GetData()->SetName("Points_XYZ");
    poly_data->SetPoints(points.GetPointer());

    // Assign for each cell vertex indices
    vtkNew<vtkIdTypeArray> cells;
    cells->SetNumberOfValues(num_points * 2);
    vtkIdType *ids = cells->GetPointer(0);
    for (vtkIdType i = 0; i < num_points; ++i) {
        ids[i * 2] = 1; // num points in the cell = 1
        ids[i * 2 + 1] = i; // pid
    }
    vtkSmartPointer<vtkCellArray> cellArray = vtkSmartPointer<vtkCellArray>::New();
    cellArray->SetCells(num_points, cells.GetPointer());
    poly_data->SetVerts(cellArray);

    // Construct a Scalar field (to see colors)
    vtkNew<vtkFloatArray> scalar_values;
    scalar_values->Allocate(num_points);
    scalar_values->SetNumberOfTuples(num_points);
    scalar_values->SetName("Z");
    poly_data->GetPointData()->AddArray(scalar_values);

    // Generate the points randomly from 3 different planes
    for (auto k(0); k < num_primitives; ++k) {
        auto generate_point = [k]() {
            Eigen::Vector3f random = Eigen::Vector3f::Random();
            random[k] = 0;
            return random;
        };
        for (auto i(0); i < num_points_per_primitive; ++i) {
            Eigen::Vector3f random = generate_point();
            auto idx = k * num_points_per_primitive + i;
            points->SetPoint(idx, random.data());
            scalar_values->SetValue(idx, random.z());
        }
    }
    poly_data->GetPointData()->SetActiveScalars("Z");
    vtkNew<vtkPolyDataMapper> polydata_mapper;
    polydata_mapper->SetInputData(poly_data.Get());

    vtkNew<vtkActor> coneActor;
    coneActor->SetMapper(polydata_mapper);

    return coneActor;
}

// Generate a Cone Actor
auto GetConeActor() {
    vtkNew<vtkNamedColors> colors;
    vtkNew<vtkConeSource> cone;
    cone->SetHeight(3.0);
    cone->SetRadius(1.0);
    cone->SetResolution(20);
    vtkNew<vtkPolyDataMapper> coneMapper;
    coneMapper->SetInputConnection(cone->GetOutputPort());
    vtkNew<vtkActor> coneActor;
    coneActor->SetMapper(coneMapper);
    return coneActor;
}

// Get Lines Actor
auto GetLineActor() {
    const auto num_lines = 10;
    const auto num_points = num_lines + 1;

    auto poly_data = vtkSmartPointer<vtkPolyData>::New();

    // Setup the array of points
    auto vtk_points = vtkSmartPointer<vtkPoints>::New();
    vtk_points->SetDataTypeToFloat();
    vtk_points->Allocate(num_points);
    vtk_points->SetNumberOfPoints(num_points);
    vtk_points->GetData()->SetName("Points_XYZ");
    poly_data->SetPoints(vtk_points.GetPointer());

    // Assign for each cell vertex indices
    auto cell_ids = vtkSmartPointer<vtkIdTypeArray>::New();
    cell_ids->SetNumberOfValues(num_lines * 3);
    vtkIdType *ids = cell_ids->GetPointer(0);
    for (vtkIdType line_id = 0; line_id < num_lines; ++line_id) {
        auto cell_id = line_id * 3;
        ids[cell_id] = 2; // num points in the cell = 2
        ids[cell_id + 1] = line_id; // origin pid
        ids[cell_id + 2] = line_id + 1; // pid of the endpoint of the line
    }

    double scale = 5.;

    vtkSmartPointer<vtkCellArray> cellArray = vtkSmartPointer<vtkCellArray>::New();
    cellArray->SetCells(num_lines, cell_ids.GetPointer());
    poly_data->SetLines(cellArray);

    auto color_field = vtkSmartPointer<vtkFloatArray>::New();
    color_field->Allocate(num_lines);
    color_field->SetName("Scalar");
    color_field->SetNumberOfTuples(num_points);
    poly_data->GetCellData()->AddArray(color_field);

    Eigen::Vector3f previous_point = Eigen::Vector3f::Random() * scale;
    for (auto line_idx(0); line_idx < num_lines; line_idx++) {

        if (line_idx == 0)
            vtk_points->SetPoint(0, previous_point.data());

        Eigen::Vector3f new_point = Eigen::Vector3f::Random() * scale;
        vtk_points->SetPoint(line_idx + 1, new_point.data());
        previous_point = new_point;

        color_field->SetValue(line_idx, float(line_idx) / (num_lines - 1));
    }
    poly_data->GetCellData()->SetActiveScalars("Scalar");

    vtkNew<vtkPolyDataMapper> polydata_mapper;
    polydata_mapper->SetInputData(poly_data.Get());

    vtkNew<vtkActor> poly_data_actor;
    poly_data_actor->SetMapper(polydata_mapper);

    return poly_data_actor;
}

int main(int argc, char **argv) {
    auto &gui = viz3d::GUI::Instance();
    gui.AddWindow(std::make_shared<TestWindow>("Test Window"));

    {
        auto vtk_window = std::make_shared<viz3d::VTKWindow>("VTK Window");
        vtk_window->AddActor(GetConeActor());
        gui.AddWindow(vtk_window);
    }

    size_t window_id;
    {
        auto vtk_window2 = std::make_shared<viz3d::VTKWindow>("VTK Window 2");
        vtk_window2->AddActor(GetPointCloudActor());
        vtk_window2->AddActor(GetLineActor());
        window_id = gui.AddWindow(vtk_window2);
    }

    std::thread gui_thread{viz3d::GUI::LaunchMainLoop, "GUI"};
    gui_thread.join();
    gui.RemoveWindow(window_id);
    gui.ClearWindows();

    return 0;
}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <iostream>
#include <cstdio>
#include <set>
#include <algorithm>
#include <cmath>

#include <boost/thread.hpp>

#include "saveload_bz2.hpp"
#include "saveload_gz.hpp"
#include "sdr.hpp"

using namespace std;

namespace hashclash {

	std::ostream& operator<<(std::ostream& o, const sdr& n)
	{
		o << "[!";
		bool first = true;
		for (unsigned b = 0; b < 32; ++b)
		{
			if (n[b] != 0)
			{
				if (first)
					first = false;
				else
					o << ",";
				if (n[b] == -1)
					o << "-";
				o << b;
			}
		}
		o << "!]";
		return o;
	}

	std::istream& operator>>(std::istream& i, sdr& n)
	{
		n.clear();
		char c;
		if (!(i >> c)) return i;
		if (c != '[') {
			i.putback(c);
			i.setstate(std::ios::failbit);
			return i;
		}
		if (!(i >> c)) return i;
		if (c != '!') {
			i.putback(c);
			i.setstate(std::ios::failbit);
			return i;
		}
		// check for empty sdr
		if (!(i >> c)) return i;
		if (c == '!') {
			if (!(i >> c)) return i;
			if (c != ']') {
				i.putback(c);
				i.setstate(std::ios::failbit);
			}
			return i;
		}
		i.putback(c);

		char s;
		bool neg = true;
		if (!(i >> s)) return i;
		if (s != '-') {
			neg = false;
			i.putback(s);
		}
		unsigned bit;
		while (i >> bit >> c) {
			if (!(bit < 32 && (c == ',' || c == '!'))) {
				i.putback(c);
				i.setstate(std::ios::failbit);
				return i;
			}
			n.mask |= 1<<bit;
			if (!neg)
				n.sign |= 1<<bit;

			if (c == '!') break;
			neg = true;
			if (!(i >> s)) return i;
			if (s != '-') {
				neg = false;
				i.putback(s);
			}
		}
		if (!(i >> c)) return i;
		if (c != ']') {
			i.putback(c);
			i.setstate(std::ios::failbit);
			return i;
		}
		return i;
	}

	uint32 best_rotated_difference(uint32 diff, int rc)
	{
		if (diff == 0 || (rc&31)==0)
			return diff;

		int rc2 = 32 - rc;
		uint32 bound = 1 << rc2;
		uint32 bound2 = 1 << rc;
		uint32 y = diff >> rc2;
		uint32 x = diff - (y<<rc2);
		uint32 p1 = (bound-x)*y;
		uint32 d1 = ((x<<rc)|y);
		if ((y<<1) > bound2)
			d1 -= bound2;
		else
			p1 = ((bound-x)<<rc) - p1;

		if (x == 0)
			return d1;

		y += 1;
		y &= ~bound2; // erase possible carry at rc-th bit
		uint32 p2 = x*y;
		uint32 d2 = ((x<<rc)|y);
		if ((y<<1) > bound2)
			d2 -= bound2;
		else
			p2 = (x<<rc) - p2;
		if (p1 > p2)
			return d1;
		return d2;
	}

	void rotate_difference(uint32 diff, int rc, std::vector<uint32>& rotateddiff, uint32 minprob)
	{
		rotateddiff.clear();
		if (diff == 0 || (rc&31)==0) {
			rotateddiff.push_back(diff);
			return;
		}
		// now p1,p2,p3,p4 < 1 * 2^32, no overflows

		int rc2 = 32 - rc;
		uint32 bound = 1 << rc2;
		uint32 bound2 = 1 << rc;
		uint32 y = diff >> rc2;
		uint32 x = diff - (y<<rc2);

		uint32 p1 = (bound-x) * y;
		if (p1 >= minprob)
			rotateddiff.push_back( ((x<<rc)|y) - bound2 );
		uint32 p2 = ((bound-x)<<rc) - p1;
		if (p2 >= minprob)
			rotateddiff.push_back( (x<<rc)|y );

		if (x != 0) {
			y += 1;
			y &= ~bound2; // erase possible carry at rc-th bit

			uint32 p3 = x * y;
			if (p3 >= minprob)
				rotateddiff.push_back( ((x<<rc)|y) - bound2 );
			uint32 p4 = (x<<rc) - p1;
			if (p4 >= minprob)
				rotateddiff.push_back( (x<<rc)|y );
		}
	}

	void rotate_difference(uint32 diff, int rc, std::vector<std::pair<uint32,double> >& rotateddiff)
	{
		double pinv = pow(double(2),-32);

		rotateddiff.clear();
		if (diff == 0 || (rc&31)==0) {
			rotateddiff.push_back(std::pair<uint32,double>(diff,1));
			return;
		}

		int rc2 = 32 - rc;
		uint32 bound = 1 << rc2;
		uint32 bound2 = 1 << rc;
		uint32 y = diff >> rc2;
		uint32 x = diff - (y<<rc2);

		uint32 p1 = 0;
		if (y) {
			p1 = (bound-x) * y;
			rotateddiff.push_back( std::pair<uint32,double>
				( ((x<<rc)|y) - bound2, double(p1)*pinv ));
		}

		uint32 p2 = ((bound-x)<<rc) - p1;
		rotateddiff.push_back( std::pair<uint32,double>
				( (x<<rc)|y, double(p2)*pinv ));

		uint32 p3 = 0, p4 = 0;
		if (x != 0) {
			y += 1;
			y &= ~bound2; // erase possible carry at rc-th bit

			if (y) {
				p3 = x * y;
				rotateddiff.push_back( std::pair<uint32,double>
					( ((x<<rc)|y) - bound2, double(p3)*pinv ));
			}

			p4 = (x<<rc) - p3;
			rotateddiff.push_back( std::pair<uint32,double>
					( (x<<rc)|y, double(p4)*pinv ));
		}
	}

	unsigned hw_table[0x800];
	struct sdr_carry {
		std::vector< std::vector<sdr> > positive, negative;
	};
	std::vector<sdr_carry> hashclash_sc(0);
	std::vector< std::vector< std::pair<unsigned,unsigned> > > hashclash_scn(0);
	void hashclash_init_scn();

	unsigned count_sdrs(uint32 n, unsigned maxw)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		uint32 l = n & 0xFFFF;
		uint32 h1 = n - l;
		uint32 h2 = h1 + 0x10000;
		unsigned h1w = hwnaf(h1);
		unsigned h2w = hwnaf(h2);
		unsigned hw = h1w;
		if (h2w<hw) hw = h2w;

		std::vector< std::pair<unsigned,unsigned> >& vl = hashclash_scn[l];
		std::vector< std::pair<unsigned,unsigned> >& vh1 = hashclash_scn[h1>>16];
		std::vector< std::pair<unsigned,unsigned> >& vh2 = hashclash_scn[h2>>16];
		unsigned tot = 0;

		for (unsigned i = 0; i < vl.size() && i+hw <= maxw; ++i)
		{
			if (vl[i].first == 0 && vl[i].second == 0) continue;
			unsigned m = 0;
			for (unsigned j = 0; j < vh1.size() && i+j<=maxw; ++j)
				m += vh1[j].first + vh1[j].second;
			tot += vl[i].first * m;
			m = 0;
			for (unsigned j = 0; j < vh2.size() && i+j<=maxw; ++j)
				m += vh2[j].first + vh2[j].second;
			tot += vl[i].second * m;
		}
		return tot;
	}

	typedef std::vector< std::vector<sdr> > vec_vec_sdr_t;
	void table_sdrs(std::vector<sdr>& result, uint32 n, unsigned maxw)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		result.clear();
		if (maxw < hwnaf(n)) return;
		std::vector< triple<uint32,uint32,uint32> > breakn(8);
		std::vector< triple<vec_vec_sdr_t*, vec_vec_sdr_t*, vec_vec_sdr_t*> > breaknsdr(8);
		uint32 m0 = n & 0x7FF;
		uint32 n0 = n - m0;
		uint32 m1 = n0 & 0x3FFFFF;
		uint32 m2 = n0 - m1;
		breakn[0]=make_triple(m0,m1,m2);
		breakn[4]=make_triple(m0,m1,m2);
		breaknsdr[0]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[4]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[2]=make_triple(m0,m1,m2);
		breakn[6]=make_triple(m0,m1,m2);
		breaknsdr[2]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[6]=make_triple(
			&hashclash_sc[m0].positive,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].negative);

		n0 += 0x800;
		m1 = n0 & 0x3FFFFF;
		m2 = n0 - m1;
		breakn[1]=make_triple(m0,m1,m2);
		breakn[5]=make_triple(m0,m1,m2);
		breaknsdr[1]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[5]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].positive,
			&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[3]=make_triple(m0,m1,m2);
		breakn[7]=make_triple(m0,m1,m2);
		breaknsdr[3]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].positive);
		breaknsdr[7]=make_triple(
			&hashclash_sc[m0].negative,
			&hashclash_sc[m1>>11].negative,
			&hashclash_sc[m2>>21].negative);
		std::vector<sdr>::const_iterator cit, citend;
		for (unsigned l = 0; l < breakn.size(); ++l)
		{
			m0 = breakn[l].first;
			m1 = breakn[l].second;
			m2 = breakn[l].third;
			vec_vec_sdr_t& v0 = *breaknsdr[l].first;
			vec_vec_sdr_t& v1 = *breaknsdr[l].second;
			vec_vec_sdr_t& v2 = *breaknsdr[l].third;
			unsigned w0 = 0; while (w0 < v0.size() && 0 == v0[w0].size()) ++w0;
			unsigned w1 = 0; while (w1 < v1.size() && 0 == v1[w1].size()) ++w1;
			unsigned w2 = 0; while (w2 < v2.size() && 0 == v2[w2].size()) ++w2;
			unsigned w0max = maxw - w1 - w2;
			unsigned w1max = maxw - w2;
			for (unsigned i0 = w0; i0 <= w0max && i0 < v0.size(); ++i0)
			for (unsigned j0 = 0; j0 < v0[i0].size(); ++j0)
			{
				sdr temp0 = v0[i0][j0];
				for (unsigned i1 = w1; i0+i1 <= w1max && i1 < v1.size(); ++i1)
				for (unsigned j1 = 0; j1 < v1[i1].size(); ++j1)
				{
					sdr temp1 = temp0;
					temp1.mask ^= v1[i1][j1].mask << 11;
					temp1.sign ^= v1[i1][j1].sign << 11;
					for (unsigned i2 = w2; i0+i1+i2 <= maxw && i2 < v2.size(); ++i2)
					{
						cit = v2[i2].begin();
						citend = v2[i2].end();
						for (; cit != citend; ++cit)
						{
							sdr temp2 = temp1;
							temp2.mask ^= cit->mask << 21;
							temp2.sign ^= cit->sign << 21;
							result.push_back(temp2);
						}
					}
				}
			}
		}

	}

	unsigned count_sdrs(uint32 n, unsigned w, bool signpos)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		uint32 l = n & 0xFFFF;
		uint32 h1 = n - l;
		uint32 h2 = h1 + 0x10000;
		unsigned h1w = hwnaf(h1);
		unsigned h2w = hwnaf(h2);
		unsigned hw = h1w;
		if (h2w<hw) hw = h2w;

		std::vector< std::pair<unsigned,unsigned> >& vl = hashclash_scn[l];
		std::vector< std::pair<unsigned,unsigned> >& vh1 = hashclash_scn[h1>>16];
		std::vector< std::pair<unsigned,unsigned> >& vh2 = hashclash_scn[h2>>16];
		unsigned tot = 0;

		for (unsigned i = 0; i < vl.size() && i+hw <= w; ++i)
		{
			if (signpos) {
				if (w-i < vh1.size())
					tot += vl[i].first * vh1[w-i].first;
				if (w-i < vh2.size())
					tot += vl[i].second * vh2[w-i].first;
			} else {
				if (w-i < vh1.size())
					tot += vl[i].first * vh1[w-i].second;
				if (w-i < vh2.size())
					tot += vl[i].second * vh2[w-i].second;
			}
		}
		return tot;
	}

	unsigned count_sdrs(sdr n, unsigned maxw, unsigned rot)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		sdr low = n, high = n;
		high.mask >>= 32-rot; high.sign >>= 32-rot;
		low.mask &= (~uint32(0))>>rot; low.sign &= low.mask;

		uint32 highdiff = high.adddiff();
		uint32 lowdiff = low.adddiff();
		bool lowpos = (lowdiff & 0x80000000)==0;
		bool highpos = (highdiff & 0x80000000)==0;

		unsigned lowcnt[33];
		unsigned highcnt[33];
		unsigned lowhw = hwnaf(lowdiff);
		unsigned highhw = hwnaf(highdiff);
		lowdiff <<= rot;
		highdiff <<= 32-rot;
		for (unsigned i = lowhw; i+highhw <= maxw; ++i)
			lowcnt[i] = count_sdrs(lowdiff, i, lowpos);
		for (unsigned i = highhw; i+lowhw <= maxw; ++i)
			highcnt[i] = count_sdrs(highdiff, i, highpos);
		unsigned tot = 0;
		for (unsigned wl = lowhw; wl+highhw <= maxw; ++wl)
		{
			unsigned m = 0;
			for (unsigned wh = highhw; wh+wl <= maxw; ++wh)
				m += highcnt[wh];
			tot += lowcnt[wl] * m;
		}
		return tot;
	}

	void table_sdrs(std::vector<sdr>& result, uint32 n, unsigned w, bool signpos)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		result.clear();
		if (w < hwnaf(n)) return;
		std::vector< triple<uint32,uint32,uint32> > breakn(4);
		std::vector< triple<vec_vec_sdr_t*, vec_vec_sdr_t*, vec_vec_sdr_t*> > breaknsdr(4);
		uint32 m0 = n & 0x7FF;
		uint32 n0 = n - m0;
		uint32 m1 = n0 & 0x3FFFFF;
		uint32 m2 = n0 - m1;
		breakn[0]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[0]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[0]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[2]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[2]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[2]=make_triple(
				&hashclash_sc[m0].positive,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].negative);

		n0 += 0x800;
		m1 = n0 & 0x3FFFFF;
		m2 = n0 - m1;
		breakn[1]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[1]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[1]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].positive,
				&hashclash_sc[m2>>21].negative);

		m2 += 0x400000;
		breakn[3]=make_triple(m0,m1,m2);
		if (signpos)
			breaknsdr[3]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].positive);
		else
			breaknsdr[3]=make_triple(
				&hashclash_sc[m0].negative,
				&hashclash_sc[m1>>11].negative,
				&hashclash_sc[m2>>21].negative);
		std::vector<sdr>::const_iterator cit, citend;
		for (unsigned l = 0; l < breakn.size(); ++l)
		{
			m0 = breakn[l].first;
			m1 = breakn[l].second;
			m2 = breakn[l].third;
			vec_vec_sdr_t& v0 = *breaknsdr[l].first;
			vec_vec_sdr_t& v1 = *breaknsdr[l].second;
			vec_vec_sdr_t& v2 = *breaknsdr[l].third;
			unsigned w0 = 0; while (w0 < v0.size() && 0 == v0[w0].size()) ++w0;
			unsigned w1 = 0; while (w1 < v1.size() && 0 == v1[w1].size()) ++w1;
			unsigned w2 = 0; while (w2 < v2.size() && 0 == v2[w2].size()) ++w2;
			unsigned w0max = w - w1 - w2;
			unsigned w1max = w - w2;
			for (unsigned i0 = w0; i0 <= w0max && i0 < v0.size(); ++i0)
			for (unsigned j0 = 0; j0 < v0[i0].size(); ++j0)
			{
				sdr temp0 = v0[i0][j0];
				for (unsigned i1 = w1; i0+i1 <= w1max && i1 < v1.size(); ++i1)
				if (w-i0-i1 < v2.size())
				for (unsigned j1 = 0; j1 < v1[i1].size(); ++j1)
				{
					sdr temp1 = temp0;
					temp1.mask ^= v1[i1][j1].mask << 11;
					temp1.sign ^= v1[i1][j1].sign << 11;
					cit = v2[w - i0 - i1].begin();
					citend = v2[w - i0 - i1].end();
					for (; cit != citend; ++cit)
					{
						sdr temp2 = temp1;
						temp2.mask ^= cit->mask << 21;
						temp2.sign ^= cit->sign << 21;
						result.push_back(temp2);
					}
				}
			}
		}
	}

	void table_sdrs(std::vector<sdr>& result, sdr n, unsigned maxw, unsigned rot)
	{
		if (hashclash_scn.size() == 0) hashclash_init_scn();

		result.clear();
		if (maxw < hwnaf(n)) return;
		sdr low = n, high = n;
		high.mask >>= 32-rot; high.sign >>= 32-rot;
		low.mask &= (~uint32(0))>>rot; low.sign &= low.mask;

		uint32 highdiff = high.adddiff();
		uint32 lowdiff = low.adddiff();
		bool lowpos = (lowdiff & 0x80000000)==0;
		bool highpos = (highdiff & 0x80000000)==0;

		std::vector< std::vector<sdr> > lowsdrs(33);
		std::vector< std::vector<sdr> > highsdrs(33);
		unsigned lowhw = hwnaf(lowdiff);
		unsigned highhw = hwnaf(highdiff);
		lowdiff <<= rot;
		highdiff <<= 32-rot;
		for (unsigned i = lowhw; i <= maxw-highhw; ++i)
			table_sdrs(lowsdrs[i], lowdiff, i, lowpos);
		for (unsigned i = highhw; i <= maxw-lowhw; ++i)
			table_sdrs(highsdrs[i], highdiff, i, highpos);
		for (unsigned wl = lowhw; wl <= maxw-highhw; ++wl)
			for (unsigned l0 = 0; l0 < lowsdrs[wl].size(); ++l0)
			{
				sdr temp0 = lowsdrs[wl][l0];
				if (temp0.hw() != wl) throw;
				temp0.mask >>= rot;
				temp0.sign >>= rot;
				for (unsigned wh = highhw; wh+wl <= maxw; ++wh)
					for (unsigned h0 = 0; h0 < highsdrs[wh].size(); ++h0)
					{
						sdr temp1 = highsdrs[wh][h0];
						if (temp1.hw() != wh) throw;
						temp1.mask ^= temp0.mask;
						temp1.sign ^= temp0.sign;
						result.push_back(temp1);
					}
			}
	}

	boost::mutex hashclash_init_scn_mutex;
	void hashclash_init_scn()
	{
		boost::lock_guard<boost::mutex> lock(hashclash_init_scn_mutex);
		if (hashclash_scn.size()) return;
		std::vector< std::vector< std::pair<unsigned,unsigned> > > hashclash_scn2(0);
/*		try {
			load_gz(hashclash_scn2, "hashclash_scn", binary_archive);
			hashclash_scn.swap(hashclash_scn2);
		} catch(...) {
			hashclash_scn2.clear();
		}
		if (hashclash_scn.size()) return;
*/
		sdr temp(0);
		hashclash_scn2.resize(1<<16);
		for (temp.mask = 0; temp.mask < 0x10000; ++temp.mask)
			for (temp.sign = 0; temp.sign <= temp.mask; ++temp.sign)
			{
				if (temp.sign & (~temp.mask)) continue;
				unsigned w = temp.hw();
				uint32 n = temp.adddiff();
				if (n & 0x80000000) // negative
				{
					n += 0x10000;
					if (hashclash_scn2[n].size() < w+1)
						hashclash_scn2[n].resize(w+1);
					++hashclash_scn2[n][w].second;
				} else // positive
				{
					if (hashclash_scn2[n].size() < w+1)
						hashclash_scn2[n].resize(w+1);
					++hashclash_scn2[n][w].first;
				}
			}
/*
		try {
			save_gz(hashclash_scn2, "hashclash_scn", binary_archive);
		} catch (...)
		{}
*/
		hashclash_scn.swap(hashclash_scn2);
	}

	struct hashclash_sdr__init {
		hashclash_sdr__init()
		{
			init_hwtable();
			init_sc();
		}

		void init_hwtable()
		{
			for (uint32 n = 0; n < 0x800; ++n)
			{
				unsigned w = 0;
				uint32 k = n;
				while (k) {
					w += k & 1;
					k >>= 1;
				}
				hw_table[n] = w;
			}
		}

		void init_sc()
		{
			hashclash_sc.resize(0x800);
			sdr temp;
			for (temp.mask = 0; temp.mask < 0x800; ++temp.mask)
				for (temp.sign = 0; temp.sign < 0x800; ++temp.sign)
				{
					if (temp.mask != (temp.sign | temp.mask)) continue;
					uint32 n = temp.adddiff();
					unsigned w = temp.hw();
					if (n & 0x80000000) // negative
					{
						n += 0x800;
						if (hashclash_sc[n].negative.size() < w+1)
							hashclash_sc[n].negative.resize(w+1);
						hashclash_sc[n].negative[w].push_back(temp);
					} else // positive
					{
						if (hashclash_sc[n].positive.size() < w+1)
							hashclash_sc[n].positive.resize(w+1);
						hashclash_sc[n].positive[w].push_back(temp);
					}
				}
		}
	};
	hashclash_sdr__init hashclash_sdr__init__now;

	void hashclash_sdr_hpp_init()
	{
		hashclash_sdr__init here;
	}

} // namespace


#ifndef __CROSSBOW_RANDOM_GENERATORH_
#define __CROSSBOW_RANDOM_GENERATORH_

#include <boost/random.hpp>

#include <boost/random/mersenne_twister.hpp>

#include <boost/math/special_functions/next.hpp>

namespace crossbow {

	typedef boost::mt19937 rng_t;

	class CrossbowRandomGenerator {

	public:
		CrossbowRandomGenerator (unsigned int);

		void randomUniformFill  (float *, const int, const float, const float);
		void randomGaussianFill (float *, const int, const float, const float, const int);

		void dump ();

	private:

	protected:
		float nextafter (const float);

		unsigned int seed;
		rng_t *rng;
	};

} /* namespace crossbow */

#endif /* __CROSSBOW_RANDOM_GENERATORH_ */


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/greater.hpp>
#include <boost/hana/less.hpp>
#include <boost/hana/string.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(
        BOOST_HANA_STRING("abc") < BOOST_HANA_STRING("bcd")
    );

    BOOST_HANA_CONSTANT_CHECK(
        BOOST_HANA_STRING("abcd") > BOOST_HANA_STRING("abc")
    );
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_LINALG_DETAILS_BLAS_ROT_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_DETAILS_BLAS_ROT_HPP_INCLUDED

#include <complex>
#include <boost/preprocessor/cat.hpp>
#include <nt2/toolbox/linalg/details/blas/blas1.hpp>
#include <nt2/toolbox/linalg/details/utility/f77_wrapper.hpp>

namespace nt2 { namespace details
{
#define NT2_ROT(T, PREFIX)                                          \
inline void rotg( const T *a, const T *b, const T *c, const T *s )  \
{                                                                   \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,rotg))(a, b, c, s);               \
}                                                                   \
/**/

  // INTERNAL ONLY
  // rotg
  // SUBROUTINE _ROTG ( A, B, C, S ) S, D
  NT2_ROT(double, d )
  NT2_ROT(float,  s )

#undef NT2_ROT

#define NT2_ROT(T, PREFIX)                                  \
inline void rotmg ( const T *d1, const T *d2                \
                  , const T *a, const T *b, const T *param  \
                  )                                         \
{                                                           \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,rotmg))(d1,d2,a,b,param); \
}                                                           \
/**/

  // INTERNAL ONLY
  // rotmg
  // SUBROUTINE _ROTMG( D1, D2, A, B, PARAM ) S, D
  NT2_ROT(double, d, )
  NT2_ROT(float,  s, )
  NT2_ROT(std::complex<double>, z, u)
  NT2_ROT(std::complex<float>,  c, u)
  NT2_ROT(std::complex<double>, z, c)
  NT2_ROT(std::complex<float>,  c, c)

#undef NT2_ROT

#define NT2_ROT(T, PREFIX)                                          \
inline void rot ( const long int *n                                 \
                , const T *x, const long int *incx                  \
                , const T *y, const long int *incy                  \
                , const T *c, const T *s                            \
                )                                                   \
{                                                                   \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,rot))(n, x, incx, y, incy, c, s); \
}                                                                   \
/**/

  // INTERNAL ONLY
  // rotmg
  // SUBROUTINE _ROT  ( N, X, INCX, Y, INCY, C, S )   S, D
  NT2_ROT(double, d)
  NT2_ROT(float,  s)
#undef NT2_ROT

#define NT2_ROT(T, PREFIX)                                            \
inline void rotm( const long int *n,                                  \
                  const T *x, const long int *incx,                   \
                  const T *y, const long int *incy,                   \
                  const T *param                                      \
                )                                                     \
{                                                                     \
  NT2_F77NAME(BOOST_PP_CAT(PREFIX,rotm))(n, x, incx, y, incy, param); \
}                                                                     \
/**/

  // INTERNAL ONLY
  // rotm
  // SUBROUTINE _ROTM ( N, X, INCX, Y, INCY, PARAM )  S, D
  NT2_ROT(double, d)
  NT2_ROT(float,  s)
#undef NT2_ROT
} }

#endif


#include <iostream>
#include <algorithm>
#include <vector>
#include <boost/array.hpp>
#include <boost/algorithm/string.hpp>

// OpenCV 特征检测模块
#include <opencv2/features2d/features2d.hpp>
// #include <opencv2/nonfree/nonfree.hpp> // use this if you want to use SIFT or SURF
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml/ml.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d.hpp>

using namespace std;
using namespace cv;

const int MAX_FEATURES = 1000;

int main(int argc, char **argv)
{
    //walk
    // cv::Mat rgb1 = cv::imread("../data/walk/walk_rgb_1.png");
    // cv::Mat label1 = cv::imread("../data/walk/walk_rgb_1_label.png", cv::IMREAD_GRAYSCALE);
    // cv::Mat depth1 = cv::imread("../data/walk/walk_depth_1.png");

    // cv::Mat rgb2 = cv::imread("../data/walk/walk_rgb_2.png");
    // cv::Mat depth2 = cv::imread("../data/walk/walk_depth_2.png");
    // cv::Mat label2 = cv::imread("../data/walk/walk_rgb_2_label.png", cv::IMREAD_GRAYSCALE);

    //person or other dynamic objects in the monitor
    cv::Mat rgb1 = cv::imread("../data/lovelive/frame0059.jpg");
    // cv::Mat label1 = cv::imread("../data/lovelive/frame0058_label.png", cv::IMREAD_GRAYSCALE);

    cv::Mat rgb2 = cv::imread("../data/lovelive/frame0060.jpg");
    // cv::Mat label2 = cv::imread("../data/lovelive/frame0059_label.png", cv::IMREAD_GRAYSCALE);

    cv::resize(rgb1, rgb1, cv::Size(640, 480));
    // cv::resize(label1, label1,  cv::Size(640, 480));
    // cv::resize(depth1, depth1, cv::Size(640, 480));

    cv::resize(rgb2, rgb2, cv::Size(640, 480));
    // cv::resize(label2, label2, cv::Size(640, 480));
    // cv::resize(depth2, depth2, cv::Size(640, 480));

    cv::Mat prevImg;
    cv::Mat currentImg;
    cv::cvtColor(rgb1, prevImg, cv::COLOR_BGR2GRAY);
    cv::cvtColor(rgb2, currentImg, cv::COLOR_RGB2GRAY);

    if(prevImg.empty() || currentImg.empty() || prevImg.channels() != 1 || currentImg.channels() != 1)
    {
        cout<<"Input Image is nullptr or the image channels is not gray!" << endl;
        system("pause");
    }

    vector<cv::KeyPoint> kpts1, kpts2;
    cv::Mat desc1, desc2;

    Ptr<cv::Feature2D> orb = ORB::create(MAX_FEATURES, 1.2, 8, 31, 0, 2, 0, 31, 20);

    orb->detectAndCompute(prevImg, cv::Mat(), kpts1, desc1);
    orb->detectAndCompute(currentImg, cv::Mat(), kpts2, desc2);

    // cv::Mat show_feature_pre;
    // rgb1.copyTo(show_feature_pre);
    // for (int i = 0; i < kpts1.size(); i++)
    // {
    //     cv::circle(show_feature_pre, kpts1[i].pt, 1, cv::Scalar(255, 0, 0), 2, 8, 0);
    // }
    // cv::namedWindow("Feature Points in Previous Image");
    // cv::imshow("Feature Points in Previous Image", show_feature_pre);

    // cv::Mat show_feature_curerent;
    // rgb2.copyTo(show_feature_curerent);
    // for (int i = 0; i < kpts2.size(); i++)
    // {
    //     cv::circle(show_feature_curerent, kpts2[i].pt, 1, cv::Scalar(0, 0, 255), 2, 8, 0);
    // }
    // cv::namedWindow("Feature Points in Current Image");
    // cv::imshow("Feature Points in Current Image", show_feature_curerent);
    // cv::waitKey(0);

    std::vector<cv::DMatch> match;
    Ptr<cv::DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    matcher->match(desc1, desc2, match, cv::Mat());

    double minDis = 9999;
    for(size_t i=0; i<match.size(); i++)
    {
        if( match[i].distance < minDis )
        {
            minDis = match[i].distance;
            cout<<minDis<<" "<<std::endl;
        }
    }

    std::vector<cv::DMatch> goodmatches; //0->static; 1->random; 2->dynamic;

    for(size_t i=0; i<match.size();i++)
    {
        if(match[i].distance < 4 * minDis)
        {
            goodmatches.push_back(match[i]);
        }
    }

    cv::Mat good_match_img;
    drawMatches(prevImg, kpts1, currentImg, kpts2, goodmatches, good_match_img);

    // cv::namedWindow("Original Previous Image");
    // cv::imshow("Original Previous Image", rgb1);

    // cv::namedWindow("Segmentation of Current Image");
    // cv::imshow("Segmentation of Current Image", rgb2);

    // cv::namedWindow("Grayscale of Previous Image");
    // cv::imshow("Grayscale of Previous Image", prevImg);
    // cv::waitKey(0);

     cv::namedWindow("Good Match");
    cv::imshow("Good Match", good_match_img);
    cv::waitKey(0);

}


#include <boost/hana/power.hpp>


#include "lineslam.h"
#include "utils.h"
#include <Eigen/StdVector>
#include <stdint.h>

#ifdef _MSC_VER
#include <unordered_set>
#else
#include <tr1/unordered_set>
#endif

#include "g2o/config.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/types/icp/types_icp.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"

#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#elif defined G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif

#include "g2o/types/slam3d/vertex_se3.h"
#include "edge_se3_lineendpts.h"
#include <fstream>
#include "../external/levmar-2.6/levmar.h"
#include "lineslam.h"
//#include <Windows.h>

using namespace Eigen;

extern cv::Mat K, distCoeffs;
extern SystemParameters sysPara;

#ifdef SLAM_LBA
void Map3d::correctAll (vector<vector<int> > lmkGidMatches)
	// use global BA to correct map+pose for loop closure
{
	// ======== landmark management ========
	for(int i=0; i<lmkGidMatches.size(); ++i) {
		int gid0 = lmkGidMatches[i][0], gid1 = lmkGidMatches[i][1]; // gid1 should be larger than gid0
		if (gid0 >= gid1) continue;

		lmklines[gid0].frmId_lnLid.insert(lmklines[gid0].frmId_lnLid.end(), lmklines[gid1].frmId_lnLid.begin(),
											lmklines[gid1].frmId_lnLid.end());
		for(int j=0; j<lmklines[gid1].frmId_lnLid.size(); ++j) {
			int fid = lmklines[gid1].frmId_lnLid[j][0];
			int lid = lmklines[gid1].frmId_lnLid[j][1];

			frames[fid].lines[lid].gid = gid0;
		}
		// delete gid1
		lmklines[gid1].frmId_lnLid.clear();
		lmklines[gid1].gid = -1;

	}

	// ======== global bundle adjustment ========
	// ----- G2O parameter setting -----
	int maxIters = 15;
	// some handy typedefs
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  MyBlockSolver;
	typedef g2o::LinearSolverCSparse<MyBlockSolver::PoseMatrixType> MyLinearSolver;

	// setup the solver
	g2o::SparseOptimizer optimizer;
	optimizer.setVerbose(false);
	MyLinearSolver* linearSolver = new MyLinearSolver();
	MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	optimizer.setAlgorithm(solver);

	// -- add the parameter representing the sensor offset  !!!!
	g2o::ParameterSE3Offset* sensorOffset = new g2o::ParameterSE3Offset;
	sensorOffset->setOffset(Eigen::Isometry3d::Identity());
	sensorOffset->setId(0);
	optimizer.addParameter(sensorOffset);

	// ----- set g2o vertices ------
	int vertex_id = 0;
	tr1::unordered_map<int,int> camvid2fid, camfid2vid, linvid2gid, lingid2vid;

	// ----- optimization parameters -----
	vector<g2o::VertexSE3*> camvertVec;
	// ---- camera pose parameters ----
	for(int i = 0; i < keyframeIdx.size(); ++i) {
		cv::Mat qi = r2q(frames[keyframeIdx[i]].R);
		Eigen::Isometry3d pose;
		Eigen:: Quaterniond q(qi.at<double>(0),qi.at<double>(1),qi.at<double>(2),qi.at<double>(3));
		pose = q;
		pose.translation() = Eigen::Vector3d(frames[keyframeIdx[i]].t.at<double>(0),
			frames[keyframeIdx[i]].t.at<double>(1),frames[keyframeIdx[i]].t.at<double>(2));
		g2o::VertexSE3 * v_se3 = new g2o::VertexSE3();
		v_se3->setId(vertex_id);
		v_se3->setEstimate(pose);
		if (i == 0 )
			v_se3->setFixed(true);
		else
			v_se3->setFixed(false);

		optimizer.addVertex(v_se3);
		camvid2fid[vertex_id] = keyframeIdx[i];
		camfid2vid[keyframeIdx[i]] = vertex_id;
		++vertex_id;
		camvertVec.push_back(v_se3);
  	}

	// ---- structure parameters ----
	vector<int> idx2Opt; // landmark idx to optimize,including fixed points
	vector<g2o::VertexLineEndpts*> linevertVec;
	linevertVec.reserve(lmklines.size());
	for(int i=0; i < lmklines.size(); ++i) {
		if(lmklines[i].gid < 0 ) continue;
		g2o::VertexLineEndpts* vln = new g2o::VertexLineEndpts();
		Eigen::VectorXd tmpLine(6);
		tmpLine<<lmklines[i].A.x, lmklines[i].A.y, lmklines[i].A.z,
		lmklines[i].B.x, lmklines[i].B.y, lmklines[i].B.z;
		vln->setEstimate(tmpLine);
		vln->setId(vertex_id);
		vln->setFixed(false);
		optimizer.addVertex(vln);
		lingid2vid[lmklines[i].gid] = vertex_id;
		linvid2gid[vertex_id] = lmklines[i].gid;
		++vertex_id;
		idx2Opt.push_back(i);
		linevertVec.push_back(vln);
	}
	// ----- set g2o edges: optimization measurements -----
	vector<g2o::EdgeSE3LineEndpts *> edgeVec;
	for(int i=0; i < idx2Opt.size(); ++i) {
		for(int j=0; j < lmklines[idx2Opt[i]].frmId_lnLid.size(); ++j) {
			int fid = lmklines[idx2Opt[i]].frmId_lnLid[j][0];
			int lid = lmklines[idx2Opt[i]].frmId_lnLid[j][1];
			if(frames[fid].isKeyFrame) {
					g2o::EdgeSE3LineEndpts * e = new g2o::EdgeSE3LineEndpts();
					assert(camfid2vid.find(fid)!=camfid2vid.end());
					e->vertices()[0] = dynamic_cast<g2o::OptimizableGraph::Vertex*>
						(optimizer.vertices().find(camfid2vid[fid])->second);
					if(e->vertices()[0]==0) {
						cerr<<"no cam vert found ... terminated \n";
						exit(0);
					}
					assert(lingid2vid.find(idx2Opt[i])!=lingid2vid.end());
					e->vertices()[1] = dynamic_cast<g2o::OptimizableGraph::Vertex*>
						(optimizer.vertices().find(lingid2vid[idx2Opt[i]])->second);
					if(e->vertices()[1]==0) {
						cerr<<"no line vert found ... terminated \n";
						exit(0);
					}
					Eigen::VectorXd tmpob(6);
					tmpob<<frames[fid].lines[lid].line3d.rndA.pos.x,frames[fid].lines[lid].line3d.rndA.pos.y,frames[fid].lines[lid].line3d.rndA.pos.z,
						   frames[fid].lines[lid].line3d.rndB.pos.x,frames[fid].lines[lid].line3d.rndB.pos.y,frames[fid].lines[lid].line3d.rndB.pos.z;
					e->setMeasurement(tmpob);
					e->information() = Eigen::MatrixXd::Identity(6,6);	// must be identity!
					cv::Mat covA = frames[fid].lines[lid].line3d.rndA.cov,
						covB = frames[fid].lines[lid].line3d.rndB.cov;
					e->endptCov = Eigen::MatrixXd::Identity(6,6);
					for(int ii=0; ii<3; ++ii) {
						for(int jj=0; jj<3; ++jj) {
							e->endptCov(ii,jj) = covA.at<double>(ii,jj);
						}
					}
					for(int ii=0; ii<3; ++ii) {
						for(int jj=0; jj<3; ++jj) {
							e->endptCov(ii+3,jj+3) = covB.at<double>(ii,jj);
						}
					}
					optimizer.addEdge(e);
					edgeVec.push_back(e);
			}
		}
	}
	optimizer.initializeOptimization();
	optimizer.computeActiveErrors();
	cout<<"gba cost " << optimizer.activeChi2();

	// ----- start g2o -----
	MyTimer timer;
	timer.start();
	optimizer.setVerbose(true);
	optimizer.optimize(maxIters);
//	optimizer.save("after.txt");
	timer.end();

	optimizer.computeActiveErrors();
	cout<<" ==> " <<optimizer.activeChi2();
	cout<<", used time: "<<timer.time_s<<" sec. \n ";
	// ----- update camera and structure parameters -----
	for(int i = 0; i < keyframeIdx.size(); ++i) {
		if(camvertVec[i]->fixed()) continue;
		if(camvid2fid.find(camvertVec[i]->id())==camvid2fid.end())
			cout<<camvertVec[i]->id()<<" vert not found ...\n";
//		cout<<"frame "<<camvid2fid[camvertVec[idx]->id()]<<" pose updated \n";
		Vector3d t = camvertVec[i]->estimate().translation();
		Quaterniond q(camvertVec[i]->estimate().rotation());
		double qd[] = {q.w(), q.x(), q.y(), q.z()};
		frames[keyframeIdx[i]].R = q2r(qd);
		frames[keyframeIdx[i]].t = (cv::Mat_<double>(3,1)<<t(0),t(1),t(2));
	}
	// ---- structure parameters ----
	for(int i=0; i < idx2Opt.size(); ++i) {
		if(! linevertVec[i]->fixed()){
			VectorXd ln = linevertVec[i]->estimate();
			lmklines[idx2Opt[i]].A.x = ln(0);
			lmklines[idx2Opt[i]].A.y = ln(1);
			lmklines[idx2Opt[i]].A.z = ln(2);
			lmklines[idx2Opt[i]].B.x = ln(3);
			lmklines[idx2Opt[i]].B.y = ln(4);
			lmklines[idx2Opt[i]].B.z = ln(5);
		}
	}
	optimizer.clear();
	write2file (*this, "_lpcls");
//	cin>>maxIters;
}
#endif


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

/**
 * Given camera parameters and noise estimates, this program
 * estimates the error that global localization via either
 * RANSAC or least-squares will have.
 **/
#include <common/init.h>
#include <common/thread.h>
#include <camera/camera_model.h>
#include <sparse_mapping/reprojection.h>

#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <opencv2/features2d/features2d.hpp>

DEFINE_bool(ransac, false,
              "Use RANSAC algorithm. If not use ceres solver.");

// Observation parameters
DEFINE_uint64(num_observations, 20,
             "Number of landmarks to observe.");
DEFINE_double(map_error, 0.01,
             "Std. dev. error of the mapped position, in m, for each dimension.");
DEFINE_double(feature_error, 0,
             "Std. dev. error from feature detection, in pixels, for each dimension.");
DEFINE_double(mismatch_prob, 0.0,
             "The probability of an observation matching to the wrong landmark.");

// Camera parameters
DEFINE_double(fov_x, 170.0,
              "Horizontal field of view in degrees.");
DEFINE_int32(xres, 640,
             "Horizontal resolution.");
DEFINE_int32(yres, 480,
             "Vertical resolution.");

// Environment parameters
DEFINE_double(visible_distance, 8.0,
             "Distance features are visible to camera.");
DEFINE_double(env_width, 6.0,
             "Width of the rectangular prism camera is inside.");
DEFINE_double(env_height, 6.0,
             "Height of the rectangular prism camera is inside.");
DEFINE_double(env_length, 20.0,
             "Length of the rectangular prism camera is inside.");
DEFINE_double(env_clutter, 0.5,
             "Distance within landmarks are located from the walls.");

// Initial error parameters
DEFINE_double(guess_angle_error, 0.04 * M_PI,
             "Std. dev. angle error of initial transform guess.");
DEFINE_double(guess_distance_error, 0.5,
             "Std. dev. distance error of initial transform guess.");

// Number of trials
DEFINE_int32(num_trials, 250,
             "Number of trials to gather statistics on");

// Ceres parameters
DEFINE_int32(max_num_iterations, 10000,
             "Maximum number of iterations for solver");

// RANSAC parameters
DEFINE_int32(ransac_inlier_tolerance, 3,
              "Maximum error in pixels for a projection to be considered correct.");
DEFINE_int32(ransac_iterations, 100,
              "Number of iterations of RANSAC.");

unsigned int rand_seed;
double rand_d() {
  return static_cast<double>(rand_r(&rand_seed)) / RAND_MAX;
}

Eigen::Vector3d EnvironmentIntersection(Eigen::Vector3d origin, Eigen::Vector3d dir) {
  double W = FLAGS_env_width / 2, H = FLAGS_env_height / 2, L = FLAGS_env_length / 2, P = FLAGS_env_clutter;
  // check each plane for intersection
  // x = -W plane
  // origin + dir * t = (-W, a, b)
  double t = (-W - origin.x()) / dir.x();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.z() >= -L && p.z() <= L && p.y() >= -H && p.y() <= H)
      return p + rand_d() * P * Eigen::Vector3d(0, 0, 1);
  }
  // x = W
  t = (W - origin.x()) / dir.x();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.z() >= -L && p.z() <= L && p.y() >= -H && p.y() <= H)
      return p + rand_d() * P * Eigen::Vector3d(0, 0, -1);
  }
  // y = -H
  t = (-H - origin.y()) / dir.y();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.x() >= -W && p.x() <= W && p.z() >= -L && p.z() <= L)
      return p + rand_d() * P * Eigen::Vector3d(0, 1, 0);
  }
  // y = H
  t = (H - origin.y()) / dir.y();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.x() >= -W && p.x() <= W && p.z() >= -L && p.z() <= L)
      return p + rand_d() * P * Eigen::Vector3d(0, -1, 0);
  }
  // z = -L
  t = (-L - origin.z()) / dir.z();
  if (t >= 0) {
    Eigen::Vector3d p = origin + t * dir;
    if (p.x() >= -W && p.x() <= W && p.y() >= -H && p.y() <= H)
      return p + rand_d() * P * Eigen::Vector3d(0, 0, 1);
  }
  // z = L
  t = (L - origin.z()) / dir.z();
  Eigen::Vector3d p = origin + t * dir;
  return p + rand_d() * P * Eigen::Vector3d(0, 0, -1);
}

Eigen::Vector3d RandomLandmark(std::default_random_engine gen) {
  double W = FLAGS_env_width / 2, H = FLAGS_env_height / 2, L = FLAGS_env_length / 2, P = FLAGS_env_clutter;
  std::uniform_int_distribution<int> face(0, 5);
  std::uniform_real_distribution<double> uniform(-1.0, 1.0);
  std::uniform_real_distribution<double> clutter(0.0, P);
  int f = face(gen);
  if (f == 0)
    return Eigen::Vector3d(uniform(gen) * W, uniform(gen) * H, L - clutter(gen));
  else if (f == 1)
    return Eigen::Vector3d(uniform(gen) * W, uniform(gen) * H, -L + clutter(gen));
  else if (f == 2)
    return Eigen::Vector3d(uniform(gen) * W, H - clutter(gen), uniform(gen) * L);
  else if (f == 3)
    return Eigen::Vector3d(uniform(gen) * W, -H + clutter(gen), uniform(gen) * L);
  else if (f == 4)
    return Eigen::Vector3d(W - clutter(gen), uniform(gen) * H, uniform(gen) * L);
  return Eigen::Vector3d(-W + clutter(gen), uniform(gen) * H, uniform(gen) * L);
}

void GenerateLandmarkObservations(const camera::CameraModel & camera,
                                  std::vector<Eigen::Vector3d>* pid_to_xyz,
                                  std::vector<Eigen::Vector2d>* observations) {
  std::default_random_engine gen;
  std::normal_distribution<double> map_error(0.0, FLAGS_map_error);
  std::normal_distribution<double> feature_error(0.0, FLAGS_feature_error);
  std::uniform_real_distribution<double> uniform(0.0, 1.0);
  while (pid_to_xyz->size() < FLAGS_num_observations) {
    Eigen::Vector3d ray = camera.Ray(rand_d() * FLAGS_xres, rand_d() * FLAGS_yres);
    Eigen::Vector3d landmark = EnvironmentIntersection(camera.GetPosition(), ray);
    if (!camera.IsInFov(landmark) ||
        (landmark - camera.GetPosition()).norm() > FLAGS_visible_distance)
      continue;
    Eigen::Vector3d map_landmark =
      landmark + Eigen::Vector3d(map_error(gen), map_error(gen), map_error(gen));
    // Purposely introducing rounding error here.
    Eigen::Vector2i image_coords = (camera.ImageCoordinates(landmark).array() + 0.5).cast<int>();
    Eigen::Vector2d obs(image_coords.x() + feature_error(gen), image_coords.y() + feature_error(gen));

    if (FLAGS_mismatch_prob > 0.0 && uniform(gen) <= FLAGS_mismatch_prob)
      map_landmark = RandomLandmark(gen);
    pid_to_xyz->push_back(map_landmark);
    observations->push_back(obs);
  }
}

void EstimateRandomCameraError(double* dist_error, double* angle_error) {
  // initialize random camera model with specified parameters
  double C = 4 * FLAGS_env_clutter;
  double W = FLAGS_env_width - 2 * C, H = FLAGS_env_height - 2 * C, L = FLAGS_env_length - 2 * C;
  Eigen::Vector3d true_camera_pos(rand_d() * W - W / 2, rand_d() * H - H / 2, rand_d() * L - L / 2);
  Eigen::Matrix3d true_camera_rotation;
  true_camera_rotation = Eigen::AngleAxisd(rand_d() * M_2_PI, Eigen::Vector3d::UnitX())
                       * Eigen::AngleAxisd(rand_d() * M_2_PI, Eigen::Vector3d::UnitY())
                       * Eigen::AngleAxisd(rand_d() * M_2_PI, Eigen::Vector3d::UnitZ());
  camera::CameraModel camera(true_camera_pos, true_camera_rotation,
                     FLAGS_fov_x * M_PI / 180.0, FLAGS_xres, FLAGS_yres);

  // create landmarks in field of view and simulate observations
  std::vector<Eigen::Vector3d> pid_to_xyz;  // landmark locations
  std::vector<Eigen::Vector2d> observations;  // observed camera coordinates of each landmark
  GenerateLandmarkObservations(camera, &pid_to_xyz, &observations);

  // create initial pose estimate
  std::default_random_engine gen;
  std::normal_distribution<double> guess_dist_err(0.0, FLAGS_guess_distance_error);
  std::normal_distribution<double> guess_angle_err(0.0, FLAGS_guess_angle_error);
  Eigen::Vector3d guess_pos(true_camera_pos);
  guess_pos += Eigen::Vector3d(guess_dist_err(gen), guess_dist_err(gen), guess_dist_err(gen));
  Eigen::Matrix3d guess_rotation(true_camera_rotation);
  guess_rotation = guess_rotation * Eigen::AngleAxisd(guess_angle_err(gen), Eigen::Vector3d::UnitX())
                    * Eigen::AngleAxisd(guess_angle_err(gen), Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(guess_angle_err(gen), Eigen::Vector3d::UnitZ());
  camera::CameraModel guess(guess_pos, guess_rotation, camera.GetParameters());

  if (!FLAGS_ransac) {
    // Solve the problem
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::ITERATIVE_SCHUR;
    options.num_threads = FLAGS_num_threads;
    options.max_num_iterations = FLAGS_max_num_iterations;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    sparse_mapping::EstimateCamera(&guess, &pid_to_xyz, observations, options, &summary);
  } else {
    sparse_mapping::RansacEstimateCamera(pid_to_xyz, observations, FLAGS_ransac_iterations,
                                         FLAGS_ransac_inlier_tolerance, &guess);
  }

  Eigen::Vector3d orig_angle = camera.GetRotation() * Eigen::Vector3d::UnitX();
  Eigen::Vector3d observed_angle = guess.GetRotation() * Eigen::Vector3d::UnitX();

  // set errors
  *dist_error = (true_camera_pos - guess.GetPosition()).norm();
  *angle_error = acos(observed_angle.dot(orig_angle));
}

int main(int argc, char** argv) {
  common::InitFreeFlyerApplication(&argc, &argv);
  rand_seed = time(NULL);

  double dist_error_mean = 0.0, dist_error_mean_2 = 0.0;
  double angle_error_mean = 0.0, angle_error_mean_2 = 0.0;
  for (int i = 0; i < FLAGS_num_trials; i++) {
    double dist_error = 0.0, angle_error = 0.0;
    EstimateRandomCameraError(&dist_error, &angle_error);
    dist_error_mean += dist_error;
    dist_error_mean_2 += dist_error * dist_error;
    angle_error_mean += angle_error;
    angle_error_mean_2 += angle_error * angle_error;
  }
  dist_error_mean    /= FLAGS_num_trials;
  angle_error_mean   /= FLAGS_num_trials;
  dist_error_mean_2  /= FLAGS_num_trials;
  angle_error_mean_2 /= FLAGS_num_trials;
  double dist_std_dev = sqrt(dist_error_mean_2 - dist_error_mean * dist_error_mean);
  double angle_std_dev = sqrt(angle_error_mean_2 - angle_error_mean * angle_error_mean);
  std::cout << "Distance Error: " << (dist_error_mean * 100.0) << " +/- " <<
    (dist_std_dev * 100.0) << " cm" << "\n";
  std::cout << "Angle Error: " << (angle_error_mean * 180.0 / M_PI) << " +/- " <<
    (angle_std_dev * 180.0 / M_PI) << " degrees" << "\n";

  return 0;
}