#ifndef STEREO_PROCESSOR_HPP
#define STEREO_PROCESSOR_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/plot.hpp>
#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <pcl_ros/point_cloud.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
#include <ctime>
#include <cmath>
#include <thread>
#include <queue>
#include <chrono>

#include "dsvo/dsvoConfig.h"
#include "data.hpp"
#include "state.hpp"
#include "helper.hpp"
#include "pose_estimater.hpp"
#include "reconstructor.hpp"
// #include "klt.hpp"
#include "scale_optimizer.hpp"
// #include "local_KF_optimizer.hpp"

#define MULTI_THREAD false

typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;

class StereoProcessor
{
private:
  State state;

  // parameters
  int OF_size = 11;       // optical flow size
  float MAX_VEL = 30;      // max pose propagation velocity

  int BLUR_SZ;            // blur size
  double BLUR_VAR;        // blur value
  int FEATURE_BLOCKS;     // divide img to blocks for feature detection
  int FEATURE_THRES;      // FAST threshold
  int FEATURES_PER_CELL;  // # of features in each block cell
  int FEATURE_OF_PYMD;    // feature tracking pyramid
  int PROP_PYMD;          // pose propagation pyramid
  int PROP_MAX_STEP;      // max step for pose propagation
  double PROP_PROJ_DIST;  // max projection distance for pose propagation
  double PROP_PTS_RATIO;  // minimal inlier ratio for successful pose propagation
  int PROP_MIN_POINTS;    // min # of features for pose propagation
  double KF_DIST;         // minimal Keyframe distance
  double BA_INLIER_THRES; // minimal inlier ratio for successful bundle adjustment
  double BA_REPROJ_DIST;  // bundle adjustment max reprojection distance
  int BA_MAX_STEP;        // max step for bundle adjustment
  int SCALE_PYMD;         // scale optimization pyramid
  int SCALE_MAX_STEP;     // max step for scale optimization
  // bool LOOP_CLOSURE;      // flag for local loop detection and optimization
  // bool REFINE_PIXEL;      // refine stereo match after scale optimization
  bool DEBUG;             // debug mode, inspecting frame by frame
  bool TEST_STEREO;       // test pure stereo match mode

  // ros modules
  ros::NodeHandle nh;
  message_filters::Subscriber<sensor_msgs::Image> *cam0_sub;
  message_filters::Subscriber<sensor_msgs::Image> *cam1_sub;
  message_filters::Synchronizer<StereoSyncPolicy> *sync;
  ros::Publisher pcl_pub;
  dynamic_reconfigure::Server<dsvo::dsvoConfig> server;
  dynamic_reconfigure::Server<dsvo::dsvoConfig>::CallbackType f;
  void imageMessageCallback(const sensor_msgs::ImageConstPtr& img0_cptr, const sensor_msgs::ImageConstPtr& img1_cptr);

  // stereo components
  CameraModel cam0;
  CameraModel cam1;
  cv::Mat cur_img0;
  cv::Mat cur_img1;
  Frame last_frame;
  KeyFrame* lastKF;
  std::vector<KeyFrame> keyframes;
  // Frame last_frame1;
  // std::vector<KeyFrame> keyframes1;
  bool param_changed;

  // camera pose between consecutive frames
  Eigen::Matrix3d R_last_frame;
  Eigen::Vector3d t_last_frame;

  // components for testing
  std::ofstream time_ofs;
  PointCloud::Ptr point_cloud;
  bool stereo_match_flag;     // stereo match is called if dsvo failed to generate points

  // record times
  double init_time=-1.0;
  double last_time;
  double cur_time;

  // core dsvo modules
  PoseEstimater pose_estimater;
  Reconstructor reconstructor;
  ScaleOptimizer scale_optimizer;
  // KLT klt;
  // LocalKFOptimizer local_KF_optimizer;
  void track(const cv::Mat& _cur_img0, const cv::Mat& _cur_img1, double _cur_time);
  void monoTrack();
   bool propagateState();
   void featureTrack(const cv::Mat& cur_img);
  bool reconstructAndOptimize(KFData& kf_data, Pose& cur_pose, FeaturePoints& curKF_fts_pts, std::vector<bool>& curKF_new_pts_flags);

  // helper functions
  KeyFrame createKeyFrame(const Pose& cur_pose,
                          const FeaturePoints& feature_points=FeaturePoints(), const std::vector<bool>& new_pts_flags=std::vector<bool>());
  void detectFeatures(const cv::Mat& img, std::vector<cv::KeyPoint>& kps, int fts_per_cell);
  void triangulateByStereoMatch(KeyFrame& keyframe);

  // frames to be featureTrack
  bool thread_featureTrack_running;
  std::queue<cv::Mat> featureFrameQueue;
  std::thread thread_featureTrack;
  void featureTrackThread();

public:
  StereoProcessor();
  ~StereoProcessor();

  void updateConfig(dsvo::dsvoConfig &config, uint32_t level);

};

#endif


#ifndef FSTINDEXEDMATRIX_H
#define FSTINDEXEDMATRIX_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    indexed_matrix.hpp
   \brief   Matrix representation and operations, allows operation in selected subspace
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz).
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <limits>
#include <cmath>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "indexed_vector.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
class Indexed_Vector;

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
class Indexed_Matrix;

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
std::ostream& operator<<(std::ostream& os, const Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>& mr);

//! Matrix representation and operations, allows operation in selected subspace
template<typename DATATYPE, typename DIMTYPE, class SUBSET>
class Indexed_Matrix {
public:
	typedef boost::shared_ptr<SUBSET> PSubset;
	Indexed_Matrix() : _n_max(0), _n(0), _sign(0), _d(0) {notify("Indexed_Matrix (empty) constructor");}
	Indexed_Matrix(const DIMTYPE n, bool LUstorage=false) : _n_max(0), _n(0), _sign(0), _d(0) {notify("Indexed_Matrix constructor"); reset(n,LUstorage);}
	Indexed_Matrix(const Indexed_Matrix& im); // copy-constructor
	Indexed_Matrix& operator=(Indexed_Matrix const &im);
	~Indexed_Matrix() {notify("Indexed_Matrix destructor");}
	void reset(const DIMTYPE n_max, bool LUstorage=false); //!< reallocates
	void redim(const DIMTYPE n=0); //!< keeps main allocations, just resets index arrays and adjusts _n; n==0 here equals _n_max

	DATATYPE& at_raw(const DIMTYPE row, const DIMTYPE col) {assert(row>=0 && row<_n); assert(col>=0 && col<_n); assert(_data); return _data[row*_n+col];}
	DATATYPE& at(const DIMTYPE row, const DIMTYPE col) {assert(row>=0 && row<_d); assert(col>=0 && col<_d); assert(_index); assert(_data); return _data[_index[row]*_n+_index[col]];}
	DATATYPE* get_row_raw(const DIMTYPE row) {assert(row>=0 && row<_n); assert(_data); return &(_data[row*_n]);}
	DATATYPE* get_row(const DIMTYPE row) {assert(row>=0 && row<_d); assert(_index); assert(_data); return &(_data[_index[row]*_n]);}

	void narrow_to(const PSubset sub); //!< fill _index[] -> access to data is then mapped to sub-matrix
	void denarrow(); //!< reset _index[] -> access is reset to the full matrix
	DIMTYPE get_n_max() const {return _n_max;}
	DIMTYPE get_n() const {return _n;}
	DIMTYPE get_d() const {return _d;}
	//void swap_rows(const IDXTYPE row1, const IDXTYPE row2);
	//void swap_cols(const IDXTYPE col1, const IDXTYPE col2);

	// "raw" operations, ignoring index[]
	void set_all_raw_to(const DATATYPE val);
	void add_raw(const Indexed_Matrix& mr);
	void add_constmul_raw(const Indexed_Matrix& mr, const DATATYPE mul);
	void copy_constmul_raw(const Indexed_Matrix& mr, const DATATYPE mul);
	void copy_raw(const Indexed_Matrix& mr);

	// "soft" methods - assumes source to be indexed sub-matrix, writes non-indexed (raw) result
	void LUdecompose(Indexed_Matrix &result); //!< returns matrix+vector+sign
	void solve_equations(Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> &result_vec, const Indexed_Matrix &LU_matrix, Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> &coef_vec); //!< returns vector
	void invert(Indexed_Matrix &result, const Indexed_Matrix &LU_matrix); //!< returns matrix
	DATATYPE determinant(const Indexed_Matrix &LU_matrix); //!< returns scalar

	friend std::ostream& operator<< <>(std::ostream& os, const Indexed_Matrix& mr);
protected:
	// NOTE: matrix size is defined by:
	//       _n_max - hard constant representing maximum allocated space of size _n_max*_n_max
	//       _n     - "full feature set" dimensionality, _n<=_n_max. If Indexed_Matrix servers as output buffer
	//                to store matrices of various (reduced) dimensionalites, this is needed for correct functionality
	//                of at(), at_raw() etc. (both "raw" and "non-raw" matrix item access methods)
	//       _d     - "virtual subset" dimensionality, _d<=_n. Assuming the matrix holds _n*_n values (_n consecutively stored
	//                rows of _n values each), a virtual sub-matrix can be defined and accessed using at() ("non-raw" methods)
	DIMTYPE _n_max; //!< allocated space can hold _n_max*_n_max matrix
	DIMTYPE _n; //!< full feature set dimensionality
	boost::scoped_array<DATATYPE> _data; //!< raw matrix data
	// if used as LU decomposition target
	boost::scoped_array<DIMTYPE> _permut; //!< LU permutation index
	int _sign; //!< permutation sign (renamed obsolete d)

	// sub-matrix access tools
	DIMTYPE _d; //!< virtual sub-matrix (as represented by _index) dimensionality
	boost::scoped_array<DIMTYPE> _index; //!< maps virtual sub-matrix _d dimensions to original _n dimensions

	// temporary needed inside LUdecompose()
	boost::scoped_array<DATATYPE> _tmp_rowscale; //!< (lazy allocation)
	boost::scoped_array<DIMTYPE> _tmp_whereis; //!< (lazy allocation)
	// temporaries needed inside invert()
	boost::scoped_ptr<Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> > _tmp_eq_coefs; //!< invert coefs (lazy allocation)
	boost::scoped_ptr<Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> > _tmp_eq_roots; //!< to store/pass intermediate results in invert() etc. (lazy allocation)
};

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::Indexed_Matrix(const Indexed_Matrix& im) : // copy-constructor
	_n_max(im._n_max),
	_n(im._n),
	_sign(im._sign),
	_d(im._d)
{
	notify("Indexed_Matrix copy-constructor.");
	if(_n_max>0)
	{
		_data.reset(new DATATYPE[_n_max*_n_max]); memcpy((void *)_data.get(),(void *)(im._data).get(),sizeof(DATATYPE)*_n_max*_n_max);
		_index.reset(new DIMTYPE[_n_max]); memcpy((void *)_index.get(),(void *)(im._index).get(),sizeof(DIMTYPE)*_n_max); // selected features (subspace) - index into _data
		if(im._permut)       {_permut.reset(new DIMTYPE[_n_max]); memcpy((void *)_permut.get(),(void *)(im._permut).get(),sizeof(DIMTYPE)*_n_max);}
		if(im._tmp_whereis)  {_tmp_whereis.reset(new DIMTYPE[_n_max]); memcpy((void *)_tmp_whereis.get(),(void *)(im._tmp_whereis).get(),sizeof(DIMTYPE)*_n_max);}
		if(im._tmp_rowscale)	{_tmp_rowscale.reset(new DATATYPE[_n_max]); memcpy((void *)_tmp_rowscale.get(),(void *)(im._tmp_rowscale).get(),sizeof(DATATYPE)*_n_max);}
	}
	if(im._tmp_eq_roots) _tmp_eq_roots.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*(im._tmp_eq_roots))); // solve_equations result
	if(im._tmp_eq_coefs)	_tmp_eq_coefs.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*(im._tmp_eq_coefs))); // equations' right hand side coefficients
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>& Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::operator=(Indexed_Matrix const &im)
{
	notify("Indexed_Matrix operator=.");
	assert( (im._n_max>0 && im._data && im._index) || (im._n_max==0 && !im._data && !im._index) );
	assert( !im._permut || (im._permut && im._n_max > 0) );
	assert( !im._tmp_whereis || (im._tmp_whereis && im._n_max > 0) );
	assert( !im._tmp_rowscale || (im._tmp_rowscale && im._n_max > 0) );
	if (this != &im)
	{
		_n=im._n;
		_d=im._d;
		_sign=im._sign;

		if(im._n_max>0)
		{
			if(_n_max!=im._n_max)
			{
				_data.reset(new DATATYPE[im._n_max*im._n_max]);
				_index.reset(new DIMTYPE[im._n_max]);
			}
			memcpy((void *)_data.get(),(void *)(im._data).get(),sizeof(DATATYPE)*im._n_max*im._n_max);
			memcpy((void *)_index.get(),(void *)(im._index).get(),sizeof(DIMTYPE)*im._n_max); // selected features (subspace) - index into _data

			if(im._permut)       {if(_n_max!=im._n_max) _permut.reset(new DIMTYPE[im._n_max]); memcpy((void *)_permut.get(),(void *)(im._permut).get(),sizeof(DIMTYPE)*im._n_max);} else _permut.reset();
			if(im._tmp_whereis)  {if(_n_max!=im._n_max) _tmp_whereis.reset(new DIMTYPE[im._n_max]); memcpy((void *)_tmp_whereis.get(),(void *)(im._tmp_whereis).get(),sizeof(DIMTYPE)*im._n_max);} else _tmp_whereis.reset();
			if(im._tmp_rowscale)	{if(_n_max!=im._n_max) _tmp_rowscale.reset(new DATATYPE[im._n_max]); memcpy((void *)_tmp_rowscale.get(),(void *)(im._tmp_rowscale).get(),sizeof(DATATYPE)*im._n_max);} else _tmp_rowscale.reset();

			_n_max=im._n_max;
		} else {
			_n_max=0;
			_data.reset();
			_index.reset();
			_permut.reset();
			_tmp_whereis.reset();
			_tmp_rowscale.reset();
		}
		// use assignment operator to accelerate...
		if(im._tmp_eq_roots) _tmp_eq_roots.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*(im._tmp_eq_roots))); // solve_equations result
			else _tmp_eq_roots.reset();
		if(im._tmp_eq_coefs)	_tmp_eq_coefs.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*(im._tmp_eq_coefs))); // equations' right hand side coefficients
			else _tmp_eq_coefs.reset();
	}
	return *this;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::reset(const DIMTYPE n_max, bool LUstorage)
{
	notify("Indexed_Matrix reset.");
	assert(n_max>0);
	if(n_max!=_n_max) {
		_n=_d=_n_max=n_max;
		_data.reset(new DATATYPE[_n_max*_n_max]);
		_index.reset(new DIMTYPE[_n_max]); // selected features (subspace) - index into _data
		if(LUstorage) {
			_permut.reset(new DIMTYPE[_n_max]);
			_tmp_whereis.reset(new DIMTYPE[_n_max]);
		} else {
			_permut.reset();
			_tmp_whereis.reset();
		}
		_tmp_rowscale.reset();
		_tmp_eq_coefs.reset();
		_tmp_eq_roots.reset();
	}
	_d=_n=_n_max;
	for(DIMTYPE i=0;i<_n_max;i++) _index[i]=i;
	if(LUstorage) {
		for(DIMTYPE i=0;i<_n_max;i++) _permut[i]=i;
		for(DIMTYPE i=0;i<_n_max;i++) _tmp_whereis[i]=i;
	}
	_sign=0;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::redim(const DIMTYPE n)
{
	notify("Indexed_Matrix redim.");
	assert(n>=0 && n<=_n_max);
	assert(_data);
	assert(_index);
	if(n==0) _d=_n=_n_max; else _d=_n=n;
	for(DIMTYPE i=0;i<_n;i++) _index[i]=i;
	if(_permut) for(DIMTYPE i=0;i<_n;i++) _permut[i]=i;
	if(_tmp_whereis) for(DIMTYPE i=0;i<_n;i++) _tmp_whereis[i]=i;
	_sign=0;
}

template<typename DATATYPE, typename DIMTYPE, typename SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::narrow_to(const PSubset sub) // fill _index[] -> access to data is then mapped to sub-matrix
{
	assert(_n>=sub->get_d_raw()); //enough space to store subset ?
	assert(_index);
	DIMTYPE f;
	_d=0;
	for(bool b=sub->getFirstFeature(f);b!=false;b=sub->getNextFeature(f)) _index[_d++]=f;
	assert(_d>0);
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::denarrow() // reset _index[] -> access is reset to the full matrix
{
	assert(_index);
	for(DIMTYPE i=0;i<_n;i++) _index[i]=i;
	_d=_n;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::set_all_raw_to(const DATATYPE val)
{
	assert(_data);
	for(DIMTYPE i=0;i<_n*_n;i++) _data[i]=val;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::add_raw(const Indexed_Matrix& mr)
{
	assert(_data);
	assert(mr._data);
	assert(mr.get_n()==_n);
	for(DIMTYPE i=0;i<_n*_n;i++) _data[i]+=mr._data[i];
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::add_constmul_raw(const Indexed_Matrix& mr, const DATATYPE mul)
{
	assert(_data);
	assert(mr._data);
	assert(mr.get_n()==_n);
	for(DIMTYPE i=0;i<_n*_n;i++) _data[i]+=mul * mr._data[i];
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::copy_constmul_raw(const Indexed_Matrix& mr, const DATATYPE mul)
{
	assert(_data);
	assert(mr._data);
	assert(mr.get_n()==_n);
	for(DIMTYPE i=0;i<_n*_n;i++) _data[i]=mul * mr._data[i];
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::copy_raw(const Indexed_Matrix& mr)
{
	assert(_data);
	assert(mr._data);
	assert(mr.get_n()==_n);
	for(DIMTYPE i=0;i<_n*_n;i++) _data[i]=mr._data[i];
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::LUdecompose(Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET> &result)
{
	// NOTE: 'result' raw data access assumed (result._index ignored)
	//       while this->_index is used when accessing this->_data
	notify("Indexed_Matrix::LUdecompose");
	assert(result._n==_d); // enough pre-allocated space required
	assert(result._data);
	assert(result._permut);
	assert(result._tmp_whereis);
	assert(_index);
	result.denarrow(); // no indexes needed in result, will be written in raw ordering

	if(!_tmp_rowscale) _tmp_rowscale.reset(new DATATYPE[_n_max]); // create 'implicit scaling of rows' buffer once for the rest of Indexed_Matrix life

	DATATYPE max;
	DIMTYPE idxmax=-1;
	DATATYPE pom,sum;
	DIMTYPE j,i1,i2,tmpi;
	DIMTYPE pROW,pROWK,pCOL;

	result._d=_d;
	pROW=0;
	for(i1=0;i1<_d;i1++) { // copy source sub-matrix to result buffer for further manipulations
		for(i2=0;i2<_d;i2++) {
			assert(pROW+i2<_d*_d);
			assert(_index[i1]*_n+_index[i2]<_n*_n);
			result._data[pROW+i2]=_data[_index[i1]*_n+_index[i2]];
		}
		pROW+=_d;
	}

	result._sign=1;
	/* initialize permutation vector */
	for(j=0;j<_d;j++) {result._permut[j]=j; result._tmp_whereis[j]=j;}

	/* find scaling of rows */
	pROW=0;
	for(i1=0;i1<_d;i1++)
	{
		max=0.0;
		for(j=0;j<_d;j++) {assert(pROW+j<_d*_d); if((pom=fabs(result._data[pROW+j]))>max) max=pom;}

		if(max<std::numeric_limits<DATATYPE>::min()) throw fst_error("Indexed_Matrix::LUdecompose() value too small/matrix singular or near-singular",23);

		_tmp_rowscale[i1]=1.0/max;
		pROW+=_d;
	}

	/* Crout's method */
	//pCOL=0;
	for(j=0;j<_d;j++)
	{
		//pROW=0;
		for(i1=0;i1<j;i1++)
		{
			pROW=result._tmp_whereis[i1]*_d;
			assert(pROW+j<_d*_d);
			sum=result._data[pROW+j];

			//pROWK=0;
			for(i2=0;i2<i1;i2++)
			{
				pROWK=result._tmp_whereis[i2]*_d;
				assert(pROW+i2<_d*_d);
				assert(pROWK+j<_d*_d);
				sum-=result._data[pROW+i2]*result._data[pROWK+j];
				//pROWK+=_d;
			}
			result._data[pROW+j]=sum;
			//pROW+=_d;
		}
		/* search for largest pivot */
		max=0.0;
		//pROW=j*_d;
		for(i1=j;i1<_d;i1++)
		{
			pROW=result._tmp_whereis[i1]*_d;
			assert(pROW+j<_d*_d);
			sum=result._data[pROW+j];

			//pROWK=0;
			for(i2=0;i2<j;i2++)
			{
				pROWK=result._tmp_whereis[i2]*_d;
				assert(pROW+i2<_d*_d);
				assert(pROWK+j<_d*_d);
				sum-=result._data[pROW+i2]*result._data[pROWK+j];
				//pROWK+=_d;
			}
			result._data[pROW+j]=sum;
			pom=_tmp_rowscale[i1]*fabs(sum);
			if(pom>max) {max=pom; idxmax=i1;}
			//pROW+=_d;
		}

		/* interchange rows ? */
		if(j!=idxmax)
		{
			// NOTE: row swaps just marked in _tmp_whereis

			assert(idxmax>=0 && idxmax<_d);
			assert(j>=0 && j<_d);

			pom=_tmp_rowscale[idxmax];
			_tmp_rowscale[idxmax]=_tmp_rowscale[j];
			_tmp_rowscale[j]=pom;

			i1=result._tmp_whereis[j];
			i2=result._tmp_whereis[idxmax];

			assert(i1>=0 && i1<_d);
			assert(i2>=0 && i2<_d);

			tmpi=result._permut[i1];
			result._permut[i1]=result._permut[i2];
			result._permut[i2]=tmpi;

			tmpi=result._tmp_whereis[result._permut[i1]];
			result._tmp_whereis[result._permut[i1]]=result._tmp_whereis[result._permut[i2]];
			result._tmp_whereis[result._permut[i2]]=tmpi;

			result._sign=-result._sign;
		}

		pCOL=result._tmp_whereis[j]*_d;

		//if(fabs(result._data[pCOL+j])<std::numeric_limits<DATATYPE>::min()) {result._data[pCOL+j]=std::numeric_limits<DATATYPE>::min(); notify("NOTE: too small value set to minimum representable number.");}
		//if(fabs(result._data[pCOL+j])<_safedown) throw fst_error("result._data[] value too small."); //result._data[pCOL+j]=_safedown;

		if(j<(_d-1))
		{
			assert(pCOL+j<_d*_d);
			if(fabs(result._data[pCOL+j])<=std::numeric_limits<DATATYPE>::min()) {
				if(result._data[pCOL+j]<0.0) pom=-std::numeric_limits<DATATYPE>::max();
				else pom=std::numeric_limits<DATATYPE>::max();
				notify("NOTE: division by too small a number - result set to maximum representable number.");
			} else
			pom=1.0/result._data[pCOL+j];

			//pROW=(j+1)*_d;
			for(i1=j+1;i1<_d;i1++)
			{
				pROW=result._tmp_whereis[i1]*_d;
				assert(pROW+j<_d*_d);
				result._data[pROW+j] *= pom;
				//pROW+=_d;
			}
		}

		//pCOL+=_d;
	}
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::solve_equations(Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> &result_vec, const Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET> &LU_matrix, Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> &coef_vec)
{
	// NOTE: result_vec, coef_vec, and LU_matrix raw data access assumed (*->_index ignored)
	//       while this->_index is used when accessing this->_data
	notify("Indexed_Matrix::solve_equations");
	assert(result_vec._data);
	assert(result_vec._n>=_d); // enough pre-allocated space required
	assert(coef_vec._data);
	assert(coef_vec._n>=_d); // enough pre-allocated space required
	assert(LU_matrix._n==_d);
	assert(LU_matrix._permut);
	assert(LU_matrix._tmp_whereis);

	DIMTYPE i,j;
	DATATYPE sum;
	DIMTYPE pROW;

	for(i=0;i<_d;i++) {assert(LU_matrix._tmp_whereis[i]<_d); result_vec[i]=coef_vec[LU_matrix._tmp_whereis[i]];} /* spravne serazeni b */

	//pROW=n;
	for(i=1;i<_d;i++)  /* result_vec[0] se nemeni */
	{
		pROW=LU_matrix._tmp_whereis[i]*_d;
		sum=0.0;
		for(j=0;j<i;j++) {
			assert(pROW+j<_d*_d);
			sum+=LU_matrix._data[pROW+j]*result_vec[j];
		}
		result_vec[i]-=sum;
		//pROW+=n;
	}

	//pROW=n*(n-1);
	for(i=_d;i>0;i--)
	{
		pROW=LU_matrix._tmp_whereis[i-1]*_d;
		assert(pROW+i-1<_d*_d);
		sum=0.0;
		for(j=i;j<_d;j++) sum+=LU_matrix._data[pROW+j]*result_vec[j];
		if(fabs(LU_matrix._data[pROW+i-1])<=std::numeric_limits<DATATYPE>::min()) {
			if(LU_matrix._data[pROW+i-1]*(result_vec[i-1]-sum)<0.0) result_vec[i-1]=-std::numeric_limits<DATATYPE>::max();
			else result_vec[i-1]=std::numeric_limits<DATATYPE>::max();
			notify("NOTE: division by too small number - result set to maximum representable number.");
		} else {
			result_vec[i-1]=(result_vec[i-1]-sum)/LU_matrix._data[pROW+i-1];
		}
		//pROW-=n;
	}
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
void Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::invert(Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET> &result, const Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET> &LU_Matrix)
{
	// NOTE: 'result' and 'LU_matrix' raw data access assumed (*->_index ignored)
	//       while this->_index is used when accessing this->_data
	notify("Indexed_Matrix::invert");
	assert(result._data);
	assert(result._n==_d); // enough pre-allocated space required
	assert(LU_Matrix._data);
	assert(LU_Matrix._permut);
	assert(LU_Matrix._index);
	assert(LU_Matrix._d==_d);

	if(_tmp_eq_roots) assert(_tmp_eq_roots->_n>=_d);
	else _tmp_eq_roots.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(_n_max)); // solve_equations result
	if(_tmp_eq_coefs) assert(_tmp_eq_coefs->_n>=_d);
	else _tmp_eq_coefs.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(_n_max)); // equations' right hand side coefficients

	DIMTYPE pROW;
	DIMTYPE i,j;

	for(j=0;j<_d;j++)
	{
		for(i=0;i<_d;i++) (*_tmp_eq_coefs)[i]=0.0;
		(*_tmp_eq_coefs)[j]=1.0;

		solve_equations(*_tmp_eq_roots,LU_Matrix,*_tmp_eq_coefs);

		pROW=0;
		for(i=0;i<_d;i++)
		{
			result._data[pROW+j]=(*_tmp_eq_roots)[i];
			pROW+=_d;
		}
	}
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
DATATYPE Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>::determinant(const Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET> &LU_matrix)
{
	// NOTE: 'LU_matrix' raw data access assumed (*->_index ignored)
	assert(LU_matrix._data);
	assert(LU_matrix._tmp_whereis);
	assert(LU_matrix._d==_d);

	notify("Indexed_Matrix::determinant");
	DATATYPE det;
	DIMTYPE pROW; //,step;
	DIMTYPE j;
	//LUdecompose(*_tmp_lu_matrix);

	det=LU_matrix._sign;
	//pROW=0; step=_d+1;
	for(j=0;j<_d;j++)
	{
		pROW=LU_matrix._tmp_whereis[j]*_d+j;
		assert(pROW<_d*_d);
		det*=LU_matrix._data[pROW];
		//pROW+=step;
	}
	return det;
}

template<typename DATATYPE, typename DIMTYPE, class SUBSET>
std::ostream& operator<<(std::ostream& os, const Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>& mr)
{
	assert(mr._n>0);
	assert(mr._d>0);
	assert(mr._data);
	assert(mr._index);
	os << "----------------"<<endl;
	os << "Indexed_Matrix::";
	os << "_index: "; for(DIMTYPE j=0;j<mr._d;j++) {os << mr._index[j] << " ";} os<<endl;
	os << "_data (indexed):"<<endl;
	for(DIMTYPE i=0;i<mr._d;i++) {for(DIMTYPE j=0;j<mr._d;j++) {os << mr._data[mr._index[i]*mr._n+mr._index[j]] << " ";} os << endl;}
	os << "----------------"<<endl;
	return os;
}

} // namespace
#endif // FSTINDEXEDMATRIX_H ///:~


#include "options.hpp"
#include <boost/crc.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;
using namespace cv;

Mat addFrame(cv::Mat& image, const cv::Size& blockSize);
Mat addOrientation(cv::Mat& image, const Size &blockSize);
void encodeLine(cv::Mat& dataImageLine, const cv::Size& blockSize, int bitCount, uint64_t bits);
void encode(cv::Mat& image, uint64_t id, const Size &blockSize);

int main(int argc, char* argv[])
{
    Options opt{argc, argv};

    if (opt.showOnlyHelp())
    {
        cout << opt << endl;
        return EXIT_SUCCESS;
    }

    constexpr auto BLOCK_COUNT = 12;
    Size blockSize{opt.blockWidth(), opt.blockWidth()};
    Size imageSize = blockSize * BLOCK_COUNT;

    Mat image{imageSize, CV_8UC1, Scalar{0.0}};

    auto border = addFrame(image, blockSize);
    auto data = addOrientation(border, blockSize);
    encode(data, opt.code(), blockSize);

    cv::imshow("Marker", image);
    cv::waitKey(0);

    imwrite(opt.filename(), image);
}

cv::Mat addFrame(cv::Mat& image, const cv::Size& blockSize)
{
    Mat vline{Size{image.cols, blockSize.height}, image.type(), cv::Scalar{0}};
    vline.copyTo(image(Rect{0,0, image.cols, blockSize.height}));
    vline.copyTo(image(Rect{0, image.rows - blockSize.height, image.cols, blockSize.height}));

    Mat hline{Size{blockSize.width, image.rows}, image.type(), cv::Scalar{0}};
    hline.copyTo(image(Rect{0,0, blockSize.width, image.cols}));
    hline.copyTo(image(Rect{image.rows - blockSize.width, 0, blockSize.width, image.cols}));

    auto orientationImage = image(Rect{
                                      blockSize.width, blockSize.height,
                                      image.cols - 2*blockSize.width, image.rows - 2*blockSize.height
                                  });

    return orientationImage;
}

cv::Mat addOrientation(cv::Mat& image, const cv::Size& blockSize)
{
    Mat whiteSquare{blockSize, image.type(), cv::Scalar{255.0, 255.0, 255.0}};
    whiteSquare.copyTo(image(Rect{0,0, blockSize.width, blockSize.height}));
    whiteSquare.copyTo(image(Rect{image.cols - blockSize.width,0, blockSize.width, blockSize.height}));
    whiteSquare.copyTo(image(Rect{0, image.rows - blockSize.height, blockSize.width, blockSize.height}));

    auto dataImage = image(
                Rect{
                    blockSize.width, blockSize.height,
                    image.cols - 2*blockSize.width, image.rows - 2*blockSize.height
                }
                );

    return dataImage;
}

void encode(cv::Mat& image, uint64_t id, const cv::Size& blockSize)
{
    const auto bitmask = 0xff;
    const auto bitCount = 8;

    for(int i = 0; i < 6; ++i)
    {
        auto dataImageLine = image(
                    Rect{
                        0, i * blockSize.height,
                        image.cols, blockSize.height
                    }
                    );

        auto bits = (id & (bitmask << i*bitCount)) >> i*bitCount;
        encodeLine(dataImageLine, blockSize, bitCount, bits);
    }

    boost::crc_16_type crc;
    crc.process_bytes(&id, sizeof(id));

    auto checksum = crc.checksum();

    auto crc1 = checksum & 0xFF;
    auto crc2 = checksum >> 8;

    cout << hex << "checksum is: " << checksum << "( " << crc1 << " - " << crc2 << " )" << dec << endl;

    auto crc1LineImage = image(
                Rect{
                    0, 6 * blockSize.height,
                    image.cols, blockSize.height
                }
                );

    auto crc2LineImage = image(
                Rect{
                    0, 7 * blockSize.height,
                    image.cols, blockSize.height
                }
                );

    encodeLine(crc1LineImage, blockSize, bitCount, crc1);
    encodeLine(crc2LineImage, blockSize, bitCount, crc2);
}

void encodeLine(cv::Mat& dataImageLine, const cv::Size& blockSize, int bitCount, uint64_t bits)
{
    for(int j = 0; j < bitCount; ++j)
    {
        uint64_t mask = uint64_t(1) << j;
        uint64_t bit = bits & mask;

        auto color = bit == 0 ? Scalar{0} : Scalar{255, 255, 255};
        cv::Mat square{blockSize, dataImageLine.type(), color};

        square.copyTo(
                    dataImageLine(
                        Rect{
                            j * blockSize.width, 0,
                            blockSize.width, blockSize.height}
                        )
                    );
    }
}


#pragma once

// standard libraries
#include <assert.h>
#include <Eigen/Dense>
#include <chrono>
#include <cmath>
#include <cstring>
#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <iterator>
#include <sstream>
#include <thread>
#include <vector>

// Open3D
#include <Open3D/Geometry/KDTreeFlann.h>
#include <Open3D/Geometry/PointCloud.h>
#include <Open3D/IO/ClassIO/PointCloudIO.h>

// OMPL
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/config.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

// TinyPly
#define TINYPLY_IMPLEMENTATION
#include "tinyply.h"

// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/bridges/unity_message_types.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/sensors/rgb_camera.hpp"

// ros
#include <ros/ros.h>

namespace ob = ompl::base;
namespace og = ompl::geometric;

using namespace flightlib;

namespace motion_planning {

class manual_timer {
  std::chrono::high_resolution_clock::time_point t0;
  double timestamp{0.0};

 public:
  void start() { t0 = std::chrono::high_resolution_clock::now(); }
  void stop() {
    timestamp = std::chrono::duration<double>(
                  std::chrono::high_resolution_clock::now() - t0)
                  .count() *
                1000.0;
  }
  const double &get() { return timestamp; }
};

struct float2 {
  float x, y;
};
struct float3 {
  float x, y, z;
};
struct double3 {
  double x, y, z;
};
struct uint3 {
  uint32_t x, y, z;
};
struct uint4 {
  uint32_t x, y, z, w;
};
std::vector<float3> verts;
float range = 1;
bool solution_found = false;
bool trajectory_found = false;

std::vector<float3> readPointCloud();
float3 min_bounds;
float3 max_bounds;

Eigen::Vector3d stateToEigen(const ompl::base::State *state);

std::vector<ompl::base::State *> path_;
std::vector<Eigen::Vector3d> vecs_;

void getBounds();

void plan();

bool isStateValid(const ob::State *state);

bool isInRange(float x, float y, float z);

open3d::geometry::KDTreeFlann kd_tree_;
Eigen::MatrixXd points_;
bool searchRadius(const Eigen::Vector3d &query_point, const double radius);

void executePath();

// void setupQuad();
bool setUnity(const bool render);
bool connectUnity(void);

// unity quadrotor
std::shared_ptr<Quadrotor> quad_ptr_;
std::shared_ptr<RGBCamera> rgb_camera_;
QuadState quad_state_;

// Flightmare(Unity3D)
std::shared_ptr<UnityBridge> unity_bridge_ptr_;
SceneID scene_id_{UnityScene::NATUREFOREST};
bool unity_ready_{false};
bool unity_render_{true};
RenderMessage_t unity_output_;
uint16_t receive_id_{0};

}  // namespace motion_planning


// Copyright Oleg Maximenko 2014.
// 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://github.com/svgpp/svgpp for library home page.

#pragma once

#include <svgpp/definitions.hpp>
#include <svgpp/detail/adapt_context.hpp>
#include <boost/optional.hpp>
#include <boost/noncopyable.hpp>
#include <boost/mpl/set.hpp>

namespace svgpp
{

typedef boost::mpl::set4<tag::attribute::x1, tag::attribute::y1, tag::attribute::x2, tag::attribute::y2>
  line_shape_attributes;

template<class Length>
class collect_line_attributes_adapter: boost::noncopyable
{
public:
  template<class Context>
  bool on_exit_attributes(Context & context) const
  {
    typedef detail::unwrap_context<Context, tag::length_policy> length_policy_context;
    typedef typename length_policy_context::policy length_policy_t;

    typename length_policy_t::length_factory_type & converter
      = length_policy_t::length_factory(length_policy_context::get(context));

    typename length_policy_t::length_factory_type::coordinate_type
      x1 = 0, y1 = 0, x2 = 0, y2 = 0;
    if (x1_)
      x1 = converter.length_to_user_coordinate(*x1_, tag::length_dimension::width());
    if (x2_)
      x2 = converter.length_to_user_coordinate(*x2_, tag::length_dimension::width());
    if (y1_)
      y1 = converter.length_to_user_coordinate(*y1_, tag::length_dimension::height());
    if (y2_)
      y2 = converter.length_to_user_coordinate(*y2_, tag::length_dimension::height());

    typedef detail::unwrap_context<Context, tag::basic_shapes_events_policy> basic_shapes_events;
    basic_shapes_events::policy::set_line(basic_shapes_events::get(context), x1, y1, x2, y2);
    return true;
  }

  void set(tag::attribute::x1, Length const & val) { x1_ = val; }
  void set(tag::attribute::y1, Length const & val) { y1_ = val; }
  void set(tag::attribute::x2, Length const & val) { x2_ = val; }
  void set(tag::attribute::y2, Length const & val) { y2_ = val; }

private:
  boost::optional<Length> x1_, y1_, x2_, y2_;
};

struct line_to_path_adapter
{
  template<class Context, class Coordinate>
  static void set_line(Context & context, Coordinate x1, Coordinate y1, Coordinate x2, Coordinate y2)
  {
    typedef detail::unwrap_context<Context, tag::path_events_policy> path_events;

    typename path_events::type & path_context = path_events::get(context);
    path_events::policy::path_move_to(path_context, x1, y1, tag::coordinate::absolute());
    path_events::policy::path_line_to(path_context, x2, y2, tag::coordinate::absolute());
    path_events::policy::path_exit(path_context);
  }
};

}


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014, Daichi Yoshikawa
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Daichi Yoshikawa nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Daichi Yoshikawa
 *
 *********************************************************************/

#ifndef __GL_WRAPPER_OBJECT_OBJECT_HPP
#define __GL_WRAPPER_OBJECT_OBJECT_HPP

#include <vector>
#include <GL/glut.h>
#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>
#include <gl_wrapper/render/material.hpp>

namespace gl_wrapper
{
  class Object;
  typedef boost::shared_ptr<Object> ObjectPtr;

  class Object
  {
  public:
    Object();

    void display();
    bool haveEmptyParam();

    bool assign_normal_to_vertex; // If false, assign normal to face
    bool assign_material_to_vertex; // If false, assign material to face

    class Offset
    {
    public:
      Offset() : euler_zyx(Eigen::Vector3d::Zero()), translation(Eigen::Vector3d::Zero()) {}
      Eigen::Vector3d euler_zyx;
      Eigen::Vector3d translation;
    };

    class Motion
    {
    public:
      Motion() : euler_zyx(Eigen::Vector3d::Zero()), translation(Eigen::Vector3d::Zero()) {}
      Eigen::Vector3d euler_zyx;
      Eigen::Vector3d translation;
    };

    Offset offset;
    Motion motion;

    std::vector<Eigen::Vector3d> vertex;
    std::vector< std::vector<unsigned int> > v_idx;
    std::vector<Eigen::Vector3d> normal;
    std::vector< std::vector<unsigned int> > n_idx;

    std::vector<MaterialPtr> material;
    std::vector<unsigned int> material_assignment;

    std::string name;
    ObjectPtr parent;
    ObjectPtr child;

    GLuint model_list;
  };
}

#endif /* __GL_WRAPPER_OBJECT_OBJECT_HPP */


/*
 * Software License Agreement
 *
 *  Point to plane metric for point cloud distortion measurement
 *  Copyright (c) 2016, MERL
 *
 *  All rights reserved.
 *
 *  Contributors:
 *    Dong Tian <tian@merl.com>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifdef _WIN32
#include "stdafx.h"
#endif

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <sstream>
#include <boost/program_options.hpp>

#include "geometric_distortion.h"

using namespace boost::program_options;
using namespace pcl::geometric_quality;

void printusage()
{
  cout << "pc_psnr cloud_a cloud_b [radiusTimes]" << endl;
  cout << "  default radiusTimes is 10" << endl;
}

int parseCommand( int ac, char * av[], commandPar &cPar )
{
  try {
    options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("fileA,a", value(&cPar.file1)->required(), "Input file 1") // value< string >
      ("fileB,b", value(&cPar.file2)->default_value(""), "Input file 2")
      ("saveNorm,A", value(&cPar.normFile)->default_value(""), "File name to output the normals of original point cloud")
      ("knn,k", value(&cPar.knn)->default_value(12), "Set KNN number of neighbor points for normal estimation. Default normal estimation method")
      ("rtimes,t", value(&cPar.rtimes)->default_value(0), "Guide to set radius for normal estimation, times to NN points distances")
      ("forceNormalEstimation,f", bool_switch(&cPar.force), "Force normal estimation even normals are provided")
      ("singlePass,s", bool_switch(&cPar.singlePass)->default_value(false), "Force running a single pass, where the loop is over the original point cloud")
      ("hausdorff,d", bool_switch(&cPar.hausdorff)->default_value(false), "Send the Haursdorff metric as well")
      ("point2point,1", bool_switch(&cPar.c2c_only)->default_value(false), "Force to report point-to-point metric only")
      ;

    // positional_options_description p;
    // p.add("rtimes", -1);
    variables_map vm;
    store(parse_command_line(ac, av, desc), vm);

    if (ac == 1 || vm.count("help")) { // @DT: Important too add ac == 1
      cout << "Usage: " << av[0] << " [options]\n";
      cout << desc;
      return 0;
    }

    notify(vm);                 // @DT: Report any missing parameters

    // It is wierd the variables were not set. Force the job
    cPar.file1 = vm["fileA"].as< string >();
    cPar.file2 = vm["fileB"].as< string >();
    cPar.normFile = vm["saveNorm"].as< string >();
    cPar.rtimes = vm["rtimes"].as< float >();
    if (cPar.rtimes == 0)
      cPar.knn = vm["knn"].as< int >();
    else
      cPar.knn = 0;
    cPar.singlePass = vm["singlePass"].as< bool >();
    cPar.c2c_only = vm["point2point"].as< bool >();

    // Safety check
    if (cPar.knn < 0)
    {
      cout << "Error: knn must be non-negative" << endl;
      return 0;
    }

    return 1;
  }

  catch(std::exception& e)
  {
    cout << e.what() << "\n";
    return 0;
  }

  // Confict check

}

void printCommand( commandPar &cPar )
{
  cout << "infile1: " << cPar.file1 << endl;
  cout << "infile2: " << cPar.file2 << endl;

  if ( cPar.knn > 0 )
    cout << "knn = " << cPar.knn << endl;
  else
    cout << "rtimes = " << cPar.rtimes << endl;

  if (cPar.normFile != "")
    cout << "save normals of original point cloud to: " << cPar.normFile << endl;
  cout << "force normal estimation: " << cPar.force << endl;
  if (cPar.singlePass)
    cout << "force running a single pass" << endl;

  cout << endl;
}

int main (int argc, char *argv[])
{
  commandPar cPar;
  if ( parseCommand( argc, argv, cPar ) == 0 )
    return 0;

  printCommand( cPar );

  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr incloud1(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr incloud2(new pcl::PointCloud<pcl::PointXYZRGBNormal>);

  if (readcloud(cPar.file1, *incloud1))
  {
    cout << "Error reading " << cPar.file1 << endl;
    return -1;
  }
  cout << "Reading file 1 done." << endl;

  if (cPar.file2 != "")
  {
    if (readcloud(cPar.file2, *incloud2))
    {
      cout << "Error reading " << cPar.file2 << endl;
      return -1;
    }
    cout << "Reading file 2 done." << endl;
  }

  // compute the point to plane distances, as well as point to point distances
  struct timeval t1, t2;
  gettimeofday(&t1, NULL);
  pcl::geometric_quality::qMetric qm;
  pcl::geometric_quality::computeGeometricQualityMetric(*incloud1, *incloud2, cPar, qm);
  gettimeofday(&t2, NULL);

  cout << "Job done! " << (t2.tv_sec  - t1.tv_sec) << " seconds elapsed (excluding the time to load the point clouds)." << endl;
  return 0;
}


// Copyright (c) 2018-2022 The Bitcoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <avalanche/voterecord.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

using namespace avalanche;

struct VoteRecordFixture {
    NodeId currentNodeId = -1;

    NodeId nextNodeId() {
        currentNodeId++;
        if (currentNodeId >= 8) {
            currentNodeId = 0;
        }
        return currentNodeId;
    }
};

BOOST_FIXTURE_TEST_SUITE(voterecord_tests, VoteRecordFixture)

#define REGISTER_VOTE_AND_CHECK(vr, vote, state, finalized, stale, confidence) \
    vr.registerVote(nextNodeId(), vote);                                       \
    BOOST_CHECK_EQUAL(vr.isAccepted(), state);                                 \
    BOOST_CHECK_EQUAL(vr.hasFinalized(), finalized);                           \
    BOOST_CHECK_EQUAL(vr.isStale(), stale);                                    \
    BOOST_CHECK_EQUAL(vr.getConfidence(), confidence);

BOOST_AUTO_TEST_CASE(vote_record) {
    VoteRecord vraccepted(true);

    // Check initial state.
    BOOST_CHECK_EQUAL(vraccepted.isAccepted(), true);
    BOOST_CHECK_EQUAL(vraccepted.hasFinalized(), false);
    BOOST_CHECK_EQUAL(vraccepted.isStale(), false);
    BOOST_CHECK_EQUAL(vraccepted.getConfidence(), 0);

    VoteRecord vr(false);

    // Check initial state.
    BOOST_CHECK_EQUAL(vr.isAccepted(), false);
    BOOST_CHECK_EQUAL(vr.hasFinalized(), false);
    BOOST_CHECK_EQUAL(vr.isStale(), false);
    BOOST_CHECK_EQUAL(vr.getConfidence(), 0);

    // We need to register 6 positive votes before we start counting.
    for (int i = 0; i < 6; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 0, false, false, false, 0);
    }

    // Next vote will flip state, and confidence will increase as long as we
    // vote yes.
    REGISTER_VOTE_AND_CHECK(vr, 0, true, false, false, 0);

    // A single neutral vote do not change anything.
    REGISTER_VOTE_AND_CHECK(vr, -1, true, false, false, 1);
    for (int i = 2; i < 8; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 0, true, false, false, i);
    }

    // Two neutral votes will stall progress.
    REGISTER_VOTE_AND_CHECK(vr, -1, true, false, false, 7);
    REGISTER_VOTE_AND_CHECK(vr, -1, true, false, false, 7);
    for (int i = 2; i < 8; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 0, true, false, false, 7);
    }

    // Now confidence will increase as long as we vote yes.
    for (int i = 8; i < AVALANCHE_FINALIZATION_SCORE; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 0, true, false, false, i);
    }

    // The next vote will finalize the decision.
    REGISTER_VOTE_AND_CHECK(vr, 1, true, true, false,
                            AVALANCHE_FINALIZATION_SCORE);

    // Now that we have two no votes, confidence stop increasing.
    for (int i = 0; i < 5; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 1, true, true, false,
                                AVALANCHE_FINALIZATION_SCORE);
    }

    // Next vote will flip state, and confidence will increase as long as we
    // vote no.
    REGISTER_VOTE_AND_CHECK(vr, 1, false, false, false, 0);

    // A single neutral vote do not change anything.
    REGISTER_VOTE_AND_CHECK(vr, -1, false, false, false, 1);
    for (int i = 2; i < 8; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 1, false, false, false, i);
    }

    // Two neutral votes will stall progress.
    REGISTER_VOTE_AND_CHECK(vr, -1, false, false, false, 7);
    REGISTER_VOTE_AND_CHECK(vr, -1, false, false, false, 7);
    for (int i = 2; i < 8; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 1, false, false, false, 7);
    }

    // Now confidence will increase as long as we vote no.
    for (int i = 8; i < AVALANCHE_FINALIZATION_SCORE; i++) {
        REGISTER_VOTE_AND_CHECK(vr, 1, false, false, false, i);
    }

    // The next vote will finalize the decision.
    REGISTER_VOTE_AND_CHECK(vr, 0, false, true, false,
                            AVALANCHE_FINALIZATION_SCORE);

    // Check that inflight accounting work as expected.
    VoteRecord vrinflight(false);
    for (int i = 0; i < 2 * AVALANCHE_MAX_INFLIGHT_POLL; i++) {
        bool shouldPoll = vrinflight.shouldPoll();
        BOOST_CHECK_EQUAL(shouldPoll, i < AVALANCHE_MAX_INFLIGHT_POLL);
        BOOST_CHECK_EQUAL(vrinflight.registerPoll(), shouldPoll);
    }

    // Clear various number of inflight requests and check everything behaves as
    // expected.
    for (int i = 1; i < AVALANCHE_MAX_INFLIGHT_POLL; i++) {
        vrinflight.clearInflightRequest(i);
        BOOST_CHECK(vrinflight.shouldPoll());

        for (int j = 1; j < i; j++) {
            BOOST_CHECK(vrinflight.registerPoll());
            BOOST_CHECK(vrinflight.shouldPoll());
        }

        BOOST_CHECK(vrinflight.registerPoll());
        BOOST_CHECK(!vrinflight.shouldPoll());
    }
}

// Test some cases where confidence never advances
BOOST_AUTO_TEST_CASE(stale_vote_always_inconclusive) {
    // Setup a record that is inconclusive so far
    VoteRecord vr(false);

    for (uint32_t i = 0; i < AVALANCHE_VOTE_STALE_THRESHOLD / 8; i++) {
        // Vote randomly, but such that there's always enough neutral votes to
        // not gain confidence.
        for (auto j = 0; j < 6; j++) {
            REGISTER_VOTE_AND_CHECK(vr, InsecureRand32(), false, false, false,
                                    0);
        }
        REGISTER_VOTE_AND_CHECK(vr, -1, false, false, false, 0);
        REGISTER_VOTE_AND_CHECK(vr, -1, false, false, false, 0);
    }

    // Vote record becomes stale after too many rounds of inconclusive voting
    REGISTER_VOTE_AND_CHECK(vr, -1, false, false, true, 0);
}

// Test all cases where records reach a specific confidence level and then go
// stale.
BOOST_AUTO_TEST_CASE(stale_vote_at_all_confidence_levels) {
    for (uint32_t vote = 0; vote <= 1; vote++) {
        for (uint32_t confidence = 0; confidence < AVALANCHE_FINALIZATION_SCORE;
             confidence++) {
            VoteRecord vr(!vote);

            // Prepare to increase confidence with some votes
            for (auto i = 0; i < 5; i++) {
                REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, false, 0);
            }

            // Increase to target confidence
            for (uint32_t i = 0; i < confidence; i++) {
                REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, false, i);
            }

            uint32_t remainingVotes =
                AVALANCHE_VOTE_STALE_THRESHOLD - confidence - 5;

            // Special case where staying at confidence of 1 requires a
            // different vote between agreeing votes
            if (confidence == 1) {
                REGISTER_VOTE_AND_CHECK(vr, -1, !vote, false, false, 0);
                REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, false, 1);
                remainingVotes -= 2;
            }

            // Vote neutral until stale
            if (confidence >
                AVALANCHE_VOTE_STALE_THRESHOLD / AVALANCHE_VOTE_STALE_FACTOR) {
                remainingVotes =
                    confidence * AVALANCHE_VOTE_STALE_FACTOR - confidence - 5;
            }
            for (uint32_t i = 0; i < remainingVotes; i++) {
                REGISTER_VOTE_AND_CHECK(vr, -1, !vote, false, false,
                                        confidence);
            }
            REGISTER_VOTE_AND_CHECK(vr, -1, !vote, false, true, confidence);
        }
    }
}

// Test some cases where confidence may flip flop and then goes stale.
BOOST_AUTO_TEST_CASE(stale_vote_random_then_inconclusive) {
    VoteRecord vr(false);

    for (uint32_t i = 0; i < AVALANCHE_FINALIZATION_SCORE - 14; i++) {
        // Vote randomly. Confidence changes are ok.
        vr.registerVote(nextNodeId(), InsecureRand32());
        BOOST_CHECK_EQUAL(vr.hasFinalized(), false);
        BOOST_CHECK_EQUAL(vr.isStale(), false);
    }

    // Reset confidence, no matter what it is right now
    for (uint32_t i = 0; i < 7; i++) {
        vr.registerVote(nextNodeId(), 0);
    }
    for (uint32_t i = 0; i < 7; i++) {
        vr.registerVote(nextNodeId(), 1);
    }
    BOOST_CHECK_EQUAL(vr.hasFinalized(), false);
    BOOST_CHECK_EQUAL(vr.isStale(), false);

    // Remainder of votes are neutral
    for (uint32_t i = 0;
         i < AVALANCHE_VOTE_STALE_THRESHOLD - AVALANCHE_FINALIZATION_SCORE;
         i++) {
        REGISTER_VOTE_AND_CHECK(vr, -1, false, false, false, 1);
    }

    // Vote record becomes stale after too many rounds of voting
    REGISTER_VOTE_AND_CHECK(vr, -1, false, false, true, 1);
}

// Test all cases where confidence flips as much as possible, ending at all
// possible confidence levels.
BOOST_AUTO_TEST_CASE(stale_vote_with_confidence_flips) {
    // Start testing with yes or no votes
    for (uint32_t voteInit = 0; voteInit <= 1; voteInit++) {
        // Test stalling at all confidence levels
        for (auto offset = 0; offset < AVALANCHE_FINALIZATION_SCORE; offset++) {
            uint32_t vote = voteInit;
            VoteRecord vr(!vote);
            uint32_t count = 0;

            // Offset with neutral votes
            for (auto i = 0; i < offset; i++) {
                REGISTER_VOTE_AND_CHECK(vr, -1, !vote, false, false, 0);
                count++;
            }

            // Prepare to increase confidence with some votes
            for (auto i = 0; i < 5; i++) {
                REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, false, 0);
                count++;
            }

            while (true) {
                // Increase confidence as fast as possible
                for (uint32_t i = 0; i < AVALANCHE_FINALIZATION_SCORE - 1;
                     i++) {
                    if (i <= AVALANCHE_VOTE_STALE_THRESHOLD /
                                 AVALANCHE_VOTE_STALE_FACTOR &&
                        count >= AVALANCHE_VOTE_STALE_THRESHOLD) {
                        REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, true,
                                                i);
                        goto finalsanitycheck;
                    }
                    if (i > AVALANCHE_VOTE_STALE_THRESHOLD /
                                AVALANCHE_VOTE_STALE_FACTOR &&
                        count >= i * AVALANCHE_VOTE_STALE_FACTOR) {
                        REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, true,
                                                i);
                        goto finalsanitycheck;
                    }

                    REGISTER_VOTE_AND_CHECK(vr, vote, !vote, false, false, i);
                    count++;
                }

                // Flip the vote
                if (vote++ >= 1) {
                    vote = 0;
                }

                // Reset confidence
                for (auto i = 0; i < 6; i++) {
                    if (count >= (AVALANCHE_FINALIZATION_SCORE - 1) *
                                     AVALANCHE_VOTE_STALE_FACTOR) {
                        REGISTER_VOTE_AND_CHECK(vr, vote, vote, false, true,
                                                AVALANCHE_FINALIZATION_SCORE);
                        goto finalsanitycheck;
                    }

                    REGISTER_VOTE_AND_CHECK(vr, vote, vote, false, false,
                                            AVALANCHE_FINALIZATION_SCORE - 1);
                    count++;
                }

                // If this fails, we are probably infinite looping for some
                // reason
                BOOST_CHECK(count <= AVALANCHE_FINALIZATION_SCORE *
                                         AVALANCHE_VOTE_STALE_FACTOR);
            }

        finalsanitycheck:
            BOOST_CHECK(vr.isStale());
        }
    }
}

BOOST_AUTO_TEST_CASE(duplicate_votes) {
    VoteRecord vr(true);

    // Register some votes, expecting confidence to increase
    for (auto i = 0; i < 7; i++) {
        BOOST_CHECK_EQUAL(vr.getConfidence(), 0);
        BOOST_CHECK(!vr.registerVote(nextNodeId(), 0));
    }
    BOOST_CHECK_EQUAL(vr.getConfidence(), 1);

    // Multiple duplicate votes do not advance confidence
    for (auto i = 0; i < 8; i++) {
        BOOST_CHECK(!vr.registerVote(currentNodeId, 0));
        BOOST_CHECK_EQUAL(vr.getConfidence(), 1);
    }

    // Register more votes with duplicates mixed in. Confidence should only
    // increase when duplicates are not used.
    auto expectedConfidence = 1;
    for (auto i = 0; i < 8; i++) {
        BOOST_CHECK(!vr.registerVote(currentNodeId, 0));
        BOOST_CHECK_EQUAL(vr.getConfidence(), expectedConfidence);
        for (auto j = i; j < 8; j++) {
            BOOST_CHECK(!vr.registerVote(nextNodeId(), 0));
            BOOST_CHECK_EQUAL(vr.getConfidence(), ++expectedConfidence);
        }
    }

    // Register enough votes to get just before finalization
    for (auto i = 0; i < 90; i++) {
        BOOST_CHECK(!vr.registerVote(nextNodeId(), 0));
        BOOST_CHECK_EQUAL(vr.getConfidence(), ++expectedConfidence);
    }

    // Sanity check that finalization occurs on the expected vote
    BOOST_CHECK(vr.registerVote(nextNodeId(), 0));
    BOOST_CHECK(vr.hasFinalized());
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_unord.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>

STF_CASE_TPL (" is_unord real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_unord;
  using r_t = decltype(is_unord(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(is_unord(bs::Inf<T>(), bs::Inf<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Minf<T>(), bs::Minf<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Nan<T>(), bs::Nan<T>()), r_t(true));
#endif
  STF_EQUAL(is_unord(-bs::Zero<T>(), -bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Half<T>(), bs::Half<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Mone<T>(), bs::Mone<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::One<T>(), bs::One<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Quarter<T>(), bs::Quarter<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Two<T>(), bs::Two<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Zero<T>(), bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_unord signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_unord;
  using r_t = decltype(is_unord(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_unord(bs::Mone<T>(), bs::Mone<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::One<T>(), bs::One<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Two<T>(), bs::Two<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Zero<T>(), bs::Zero<T>()), r_t(false));
}

STF_CASE_TPL (" is_unord unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_unord;
  using r_t = decltype(is_unord(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_unord(bs::One<T>(), bs::One<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Two<T>(), bs::Two<T>()), r_t(false));
  STF_EQUAL(is_unord(bs::Zero<T>(), bs::Zero<T>()), r_t(false));
}

STF_CASE ( "is_unord bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_unord;
  using r_t = decltype(is_unord(bool(), bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_unord(false, false), false);
  STF_EQUAL(is_unord(false, false), false);
  STF_EQUAL(is_unord(false, false), false);
  STF_EQUAL(is_unord(false, false), false);
}


//  Copyright John Maddock 2008.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0.  (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#define BOOST_MATH_TR1_SOURCE
#include <boost/math/tr1.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/sign.hpp>
#include "c_policy.hpp"

namespace boost{ namespace math{ namespace tr1{

template<> bool BOOST_MATH_TR1_DECL signbit<long double> BOOST_PREVENT_MACRO_SUBSTITUTION(long double x)
{
   return static_cast<bool>((boost::math::signbit)(x));
}

template<> int BOOST_MATH_TR1_DECL fpclassify<long double> BOOST_PREVENT_MACRO_SUBSTITUTION(long double x)
{
   return (boost::math::fpclassify)(x);
}

template<> bool BOOST_MATH_TR1_DECL isfinite<long double> BOOST_PREVENT_MACRO_SUBSTITUTION(long double x)
{
   return (boost::math::isfinite)(x);
}

template<> bool BOOST_MATH_TR1_DECL isinf<long double> BOOST_PREVENT_MACRO_SUBSTITUTION(long double x)
{
   return (boost::math::isinf)(x);
}

template<> bool BOOST_MATH_TR1_DECL isnan<long double> BOOST_PREVENT_MACRO_SUBSTITUTION(long double x)
{
   return (boost::math::isnan)(x);
}

template<> bool BOOST_MATH_TR1_DECL isnormal<long double> BOOST_PREVENT_MACRO_SUBSTITUTION(long double x)
{
   return (boost::math::isnormal)(x);
}

}}}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generates the -inf IEEE value (\f$-\infty\f$) for floating types and minimum value
    for integer types

    @par Header <boost/simd/constant/minf.hpp>

    @par Semantic

    For any type @c T,

    @code
    T x = Minf<T>();
    @endcode

    @return The least value for the input type
  **/
  template<typename T> T Minf();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates the -inf IEEE value (\f$-\infty\f$) for floating types and minimum value
      for integer types

      @par Semantic

      For any type @c T,

      @code
      T x = minf(as(T{}));
      @endcode

      return the minimum representable value for this type.

      @return The minimum representable value of the input type
    **/
    Value Minf();
  }
} }
#endif

#include <boost/simd/constant/scalar/minf.hpp>
#include <boost/simd/constant/simd/minf.hpp>

#endif


#pragma once

#include <boost/intrusive_ptr.hpp>
#include <memory>
#include <terark/fstring.hpp>
#include <terark/int_vector.hpp>
#include <terark/io/DataIO.hpp>
#include <terark/io/StreamBuffer.hpp>
#include <terark/util/fstrvec.hpp>
#include <terark/util/refcount.hpp>
#include <terark/valvec.hpp>

using terark::febitvec;
using terark::valvec;

namespace terark {

template <size_t P, size_t W> class rank_select_few_builder;

/* rank_select_few is a succinct rank select structure specialized for some
 * cases that one kind of element is far lesser than the other. In this case,
 * we call minor element as pivot, other element as complement, which better
 * illustrating states of elements.
 *
 * The following example is showing a case that "1" is pivot.
 * --------------------------------------------------------------------------
 * position |  0 |  1 |  2 |  3 |  4 |  5 |  6 |  7 |  8 |  9 | 10 | 11 | ...
 * --------------------------------^----------------------------------^------
 * element  |  0 |  0 |  0 |  0 |  1 |  0 |  0 |  0 |  0 |  0 |  0 |  1 | ...
 * --------------------------------^----------------------------------^------
 *     |  |
 *    \|  |/   We can store only pivots' position by rank without losing any
 *     \  /    infomation.
 *      \/
 * ---------------------------------------------------
 * pivots' rank     : logical address |  0 |  1 | ...
 * ---------------------------------------------------
 * pivots' position : data in storage |  4 | 11 | ...
 * ---------------------------------------------------
 */
template <size_t P, size_t W> class rank_select_few {
  friend class rank_select_few_builder<P, W>;

private:

  // lower_bound in pivots
  size_t lower_bound(size_t val) const;
  size_t lower_bound(size_t val, size_t &hint) const;

  // select complement element with id, i.e. rank of complement
  size_t select_complement(size_t id) const;
  size_t select_complement(size_t id, size_t &hint) const;

  // value from logical address
  inline size_t val_a_logi(size_t pos) const {
    return (*reinterpret_cast<const size_t *>(m_mempool.data() + pos * W)) &
           (W == 8 ? size_t(-1) : (1ULL << ((W * 8) & 63)) - 1);
  }

  // value of pointer
  inline size_t val_of_ptr(const uint8_t* ptr) const {
    return (*reinterpret_cast<const size_t *>(ptr)) &
           (W == 8 ? size_t(-1) : (1ULL << ((W * 8) & 63)) - 1);
  }

public:
  rank_select_few() : m_num0(), m_num1(), m_layer(), m_offset() {}

  // return true if value at pos is 1.
  bool operator[](size_t pos) const;

  // impl of hint probing
  bool at_with_hint(size_t pos, size_t &hint) const;

  // return true if value at pos is 0.
  bool is0(size_t pos) const { return !operator[](pos); }
  bool is0(size_t pos, size_t &hint) const { return !at_with_hint(pos, hint); }

  // return true if value at pos is 1.
  bool is1(size_t pos) const { return operator[](pos); }
  bool is1(size_t pos, size_t &hint) const { return at_with_hint(pos, hint); }

  // return element rank of pos
  size_t rank0(size_t pos) const;
  size_t rank0(size_t pos, size_t &hint) const;
  size_t rank1(size_t pos) const;
  size_t rank1(size_t pos, size_t &hint) const;

  // return pos of select element
  size_t select0(size_t id) const;
  size_t select0(size_t id, size_t &hint) const;
  size_t select1(size_t id) const;
  size_t select1(size_t id, size_t &hint) const;

  // return length of continuous 0-sequence of pos
  size_t zero_seq_len(size_t pos) const;
  size_t zero_seq_len(size_t pos, size_t &hint) const;

  // return length of reverse continuous 0-sequence of pos
  size_t zero_seq_revlen(size_t pos) const;
  size_t zero_seq_revlen(size_t pos, size_t &hint) const;

  // return length of continuous 1-sequence of pos
  size_t one_seq_len(size_t pos) const;
  size_t one_seq_len(size_t pos, size_t &hint) const;

  // return length of reverse continuous 1-sequence of pos
  size_t one_seq_revlen(size_t pos) const;
  size_t one_seq_revlen(size_t pos, size_t &hint) const;

  // return maximum of rank 0
  size_t max_rank0() const { return m_num0; }

  // return maximum of rank 1
  size_t max_rank1() const { return m_num1; }

  // return size of elements
  size_t size() const { return m_num0 + m_num1; }

  // return raw data pointer
  const byte_t *data() const { return m_mempool.data(); }

  // return structure occupied memory size
  size_t mem_size() const { return m_mempool.full_mem_size(); }

  // swap operator
  void swap(rank_select_few &r) {
    std::swap(m_num0, r.m_num0);
    std::swap(m_num1, r.m_num1);
    std::swap(m_offset, r.m_offset);
    std::swap(m_layer, r.m_layer);
    m_mempool.swap(r.m_mempool);
  }

  // force structuralize certain memory
  void risk_mmap_from(unsigned char *src, size_t size) {
    m_mempool.risk_set_data(src, size);
    uint64_t *ptr = reinterpret_cast<uint64_t*>(m_mempool.data() + size - 8);
    m_layer = *ptr;
    assert(m_layer < 8);
    ptr -= m_layer;
    m_offset = ptr--;
    m_num1 = *ptr--;
    m_num0 = *ptr;
  }

  // release ownership without destroying internal elements
  void risk_release_ownership() { m_mempool.risk_release_ownership(); }

  void clear() { m_mempool.clear(); }

private:
  uint64_t m_num0, m_num1, m_layer;
  uint64_t *m_offset;
  valvec<uint8_t> m_mempool;
};

/*
 * Tool class to build rank_select_few.
 * init this builder with number of zeroes and ones, reverse build or not
 * For example, to build follow structrue.
 * ----------------------------------------------------------------------
 * position |  0 |  1 |  2 |  3 |  4 |  5 |  6 |  7 |  8 |  9 | 10 | 11 |
 * --------------------------------^----------------------------------^--
 * element  |  0 |  0 |  0 |  0 |  1 |  0 |  0 |  0 |  0 |  0 |  0 |  1 |
 * --------------------------------^----------------------------------^--
 * {
 *   rank_select_few<1, 4> rs;
 *   rank_select_few_builder<1, 4> rs_builder(10, 2, false);
 *   rs_builder.insert(4);
 *   rs_builder.insert(11);
 *   rs_builder.finish(&rs);
 * }
 * in case 3rd argument is true, insertion should be descending.
 */
template <size_t P, size_t W> class rank_select_few_builder {
  friend class rank_select_few<P, W>;

public:
  // construct rs_builder with number of elements and insert direction
  rank_select_few_builder(size_t num0, size_t num1, bool rev);
  ~rank_select_few_builder() {}

  // insert ones position
  void insert(size_t pos);

  // finish building process and transport to certain rank_select_few class
  void finish(rank_select_few<P, W> *);

private:
  bool m_rev;
  uint64_t m_last;
  uint8_t *m_it;
  uint64_t m_num0, m_num1, m_layer;
  uint64_t *m_offset;
  valvec<uint8_t> m_mempool;
};

template <size_t W> using rank_select_fewzero = rank_select_few<0, W>;
template <size_t W> using rank_select_fewone = rank_select_few<1, W>;

// rank_select_few wrapper for compatibility
template<class T>
class rank_select_hint_wrapper {
  const T& rs_;
public:
  rank_select_hint_wrapper(const T& rs, size_t* /*hint*/) : rs_(rs) {}

  bool operator[](size_t pos) const { return rs_.is1(pos); }
  bool is0(size_t pos) const { return rs_.is0(pos); }
  bool is1(size_t pos) const { return rs_.is1(pos); }
  size_t rank0(size_t pos) const { return rs_.rank0(pos); }
  size_t rank1(size_t pos) const { return rs_.rank1(pos); }
  size_t select0(size_t pos) const { return rs_.select0(pos); }
  size_t select1(size_t pos) const { return rs_.select1(pos); }
  size_t zero_seq_len(size_t pos) const { return rs_.zero_seq_len(pos); }
  size_t zero_seq_revlen(size_t pos) const { return rs_.zero_seq_revlen(pos); }
  size_t one_seq_len(size_t pos) const { return rs_.one_seq_len(pos); }
  size_t one_seq_revlen(size_t pos) const { return rs_.one_seq_revlen(pos); }

  size_t max_rank0() const { return rs_.max_rank0(); }
  size_t max_rank1() const { return rs_.max_rank1(); }
  size_t size() const { return rs_.size(); }
};

// rank_select_few with hint wrapper for compatibility
template<size_t P, size_t W>
class rank_select_hint_wrapper<rank_select_few<P, W>> {
  const rank_select_few<P, W>& rs_;
  size_t* h_;
public:
  rank_select_hint_wrapper(const rank_select_few<P, W>& rs, size_t* hint) : rs_(rs), h_(hint) {
    assert(hint != nullptr);
  }

  bool operator[](size_t pos) const { return rs_.is1(pos, *h_); }
  bool is0(size_t pos) const { return rs_.is0(pos, *h_); }
  bool is1(size_t pos) const { return rs_.is1(pos, *h_); }
  size_t rank0(size_t pos) const { return rs_.rank0(pos, *h_); }
  size_t rank1(size_t pos) const { return rs_.rank1(pos, *h_); }
  size_t select0(size_t pos) const { return rs_.select0(pos, *h_); }
  size_t select1(size_t pos) const { return rs_.select1(pos, *h_); }
  size_t zero_seq_len(size_t pos) const { return rs_.zero_seq_len(pos, *h_); }
  size_t zero_seq_revlen(size_t pos) const { return rs_.zero_seq_revlen(pos, *h_); }
  size_t one_seq_len(size_t pos) const { return rs_.one_seq_len(pos, *h_); }
  size_t one_seq_revlen(size_t pos) const { return rs_.one_seq_revlen(pos, *h_); }

  size_t max_rank0() const { return rs_.max_rank0(); }
  size_t max_rank1() const { return rs_.max_rank1(); }
  size_t size() const { return rs_.size(); }
};

template<class T>
rank_select_hint_wrapper<T> make_rank_select_hint_wrapper(const T& rs, size_t* hint) {
  return rank_select_hint_wrapper<T>(rs, hint);
}

} // namespace terark


#include "stdafx.h"
#include "UTCOffset.h"

#include <boost/date_time/c_local_time_adjustor.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <sstream>

namespace systelab { namespace trace {

	boost::optional<std::string> UTCOffset::m_currentOffset = boost::none;

	UTCOffset::UTCOffset() = default;
	UTCOffset::~UTCOffset() = default;

	std::string UTCOffset::getCurrentOffset() const
	{
		if (!m_currentOffset)
		{
			m_currentOffset = calculateCurrentOffset();
		}

		return *m_currentOffset;
	}

	std::string UTCOffset::calculateCurrentOffset() const
	{
		const boost::posix_time::ptime utcTime = boost::posix_time::second_clock::universal_time();
		const boost::posix_time::ptime localTime = boost::date_time::c_local_adjustor<boost::posix_time::ptime>::utc_to_local(utcTime);
		const boost::posix_time::time_duration td = localTime - utcTime;

		const char fillChar = '0';
		const char timeSeparator = ':';

		std::stringstream out;
		out << (td.is_negative() ? '-' : '+');
		out << std::setw(2) << std::setfill(fillChar) << boost::date_time::absolute_value(td.hours());
		out << timeSeparator;
		out << std::setw(2) << std::setfill(fillChar) << boost::date_time::absolute_value(td.minutes());

		return out.str();
	}
}}


//
//  sequence.cpp
//  express
//
//  Created by Adam Roberts on 1/24/12.
//  Copyright 2012 Adam Roberts. All rights reserved.
//

#include "sequence.h"
#include <cassert>
#include <boost/math/distributions/binomial.hpp>

using namespace std;
using namespace boost::math;

string Sequence::serialize() {
  vector<char> seq;
  for (size_t i = 0; i < length(); i++) {
    if (i/4 == seq.size()) {
      seq.push_back(0);
    }
    seq.back() += operator[](i) << (2 * (i % 4));
  }

  return string(seq.begin(), seq.end());
}

SequenceFwd::SequenceFwd():  _ref_seq(NULL), _prob(0), _len(0) {}

SequenceFwd::SequenceFwd(const std::string& seq, bool rev, bool prob)
    : _prob(prob), _len(seq.length()) {
  if (prob) {
    _est_seq = FrequencyMatrix<float>(seq.length(), NUM_NUCS, 0.001);
    _obs_seq = FrequencyMatrix<float>(seq.length(), NUM_NUCS, LOG_0);
    _exp_seq = FrequencyMatrix<float>(seq.length(), NUM_NUCS, LOG_0);
  }
  set(seq, rev);
}

SequenceFwd::SequenceFwd(const SequenceFwd& other)
    : _obs_seq(other._obs_seq), _exp_seq(other._exp_seq),
      _prob(other._prob), _len(other.length()) {
  if (other._ref_seq) {
    char* ref_seq = new char[_len];
    std::copy(other._ref_seq.get(), other._ref_seq.get() + _len, ref_seq);
    _ref_seq.reset(ref_seq);
  }
}

SequenceFwd& SequenceFwd::operator=(const SequenceFwd& other) {
  if (other._ref_seq) {
    _len = other.length();
    char* ref_seq = new char[_len];
    std::copy(other._ref_seq.get(), other._ref_seq.get() + _len, ref_seq);
    _ref_seq.reset(ref_seq);
    _obs_seq = other._obs_seq;
    _exp_seq = other._exp_seq;
    _prob = other._prob;
  }
  return *this;
}

void SequenceFwd::set(const std::string& seq, bool rev) {
  char* ref_seq = new char[seq.length()];
  for (size_t i = 0; i < seq.length(); i++) {
    ref_seq[i] = (rev) ? complement(ctoi(seq[seq.length()-1-i])) : ctoi(seq[i]);
    if (_prob) {
      _est_seq.increment(i, ref_seq[i], log((float)2));
    }
  }
  _ref_seq.reset(ref_seq);
  _len = seq.length();
}

size_t SequenceFwd::operator[](const size_t index) const {
  assert(index < _len);
  if (_prob) {
    return _est_seq.argmax(index);
  }
  return _ref_seq[index];
}

size_t SequenceFwd::get_ref(const size_t index) const {
  assert(index < _len);
  //assert(_ref_seq[index] == operator[](index));
  return _ref_seq[index];
}

float SequenceFwd::get_prob(const size_t index, const size_t nuc) const {
  assert(_prob);
  return _est_seq(index, nuc);
}

float SequenceFwd::get_obs(const size_t index, const size_t nuc) const {
  assert(index < _len);
  return _obs_seq(index,nuc, false);
}

float SequenceFwd::get_exp(const size_t index, const size_t nuc) const {
  assert(index < _len);
  return _exp_seq(index,nuc, false);
}

void SequenceFwd::update_est(const size_t index, const size_t nuc, float mass) {
  assert(_prob);
  _est_seq.increment(index, nuc, mass);
}

void SequenceFwd::update_obs(const size_t index, const size_t nuc, float mass) {
  assert(_prob);
  _obs_seq.increment(index, nuc, mass);
}

void SequenceFwd::update_exp(const size_t index, const size_t nuc, float mass) {
  assert(_prob);
  _exp_seq.increment(index, nuc, mass);
}

void SequenceFwd::calc_p_vals(vector<double>& p_vals) const {
  p_vals = vector<double>(_len, 1.0);
  for (size_t i = 0; i < _len; ++i) {
    double N = round(sexp(_obs_seq.sum(i)));
    if (N<5) {
      continue;
    }
    size_t ref_nuc = get_ref(i);
    double max_obs = 0;
    for (size_t nuc = 0; nuc < NUM_NUCS; ++nuc) {
      if (nuc == ref_nuc) {
        continue;
      }

      double obs_n = round(sexp(_obs_seq(i,nuc,false)));
      max_obs = max(max_obs, obs_n);
    }

    double p_val = 0;

    for (size_t nuc = 0; nuc < NUM_NUCS; ++nuc) {
      if (nuc == ref_nuc) {
        continue;
      }

      double exp_p = sexp(_exp_seq(i, nuc));
      binomial binom(N, exp_p);
      p_val += log(cdf(binom, max_obs));
    }
    p_vals[i] -= sexp(p_val);
  }
}

void SequenceRev::calc_p_vals(vector<double>& p_vals) const
{
  vector<double> temp;
  _seq->calc_p_vals(temp);
  p_vals = vector<double>(length());
  for(size_t i = 0; i < length(); i++) {
    p_vals[i] = temp[length()-i-1];
  }
}

/*
void SequenceFwd::calc_p_vals(vector<double>& p_vals) const
{
    p_vals = vector<double>(_len, 1);
    for (size_t i = 0; i < _len; ++i)
    {
        double N = sexp(_obs_seq.sum(i));

        if (N==0)
            continue;

        size_t ref_nuc = get_ref(i);
        double p_val = LOG_0;

        vector<double> cdfs(4);
        for (size_t nuc = 0; nuc < NUM_NUCS; ++nuc)
        {
            if (nuc == ref_nuc)
                continue;

            double p = sexp(_exp_seq(i,nuc));
            double obs_n = sexp(_obs_seq(i,nuc,false));
            normal norm (N*p, sqrt(N*p*(1-p)));
            cdfs[nuc] = cdf(norm, obs_n);
        }

        for (size_t nuc1 = 0; nuc1 < NUM_NUCS; ++nuc1)
        {
            if (nuc1 == ref_nuc)
                continue;

            double term = log(1-cdfs[nuc1]);

            for(size_t nuc2 = 0; nuc2 < NUM_NUCS; ++nuc2)
            {
                if (nuc2 == nuc1 || nuc2 == ref_nuc)
                    continue;
                term += log(cdfs[nuc2]);
            }
            p_val = log_add(p_val, term);
        }

        p_vals[i] = sexp(p_val);
    }
}
*/

/*
void SequenceFwd::calc_p_vals(vector<double>& p_vals) const
{
    p_vals = vector<double>(_len, 1.0);
    boost::math::chi_squared_distribution<double> chisq(3);
    double obs_n,exp_n;
    for (size_t i = 0; i < _len; ++i)
    {
        if (_obs_seq.sum(i)==LOG_0)
            continue;

        double S = 0;
        for (size_t nuc = 0; nuc < NUM_NUCS; ++nuc)
        {
            obs_n = sexp(_obs_seq(i,nuc,false));
            exp_n = sexp(_exp_seq(i,nuc,false));
            S += (obs_n-exp_n)*(obs_n-exp_n)/exp_n;
        }

        p_vals[i] -= boost::math::cdf(chisq,S);
    }
}
 */


/**
 * @file
 * @author  Mohammad S. Babaei <info@babaei.net>
 * @version 0.1.0
 *
 * @section LICENSE
 *
 * (The MIT License)
 *
 * Copyright (c) 2016 Mohammad S. Babaei
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @section DESCRIPTION
 *
 * Provides AES and SHA512 cryptographic operations in addition to fast Base64
 * encoding or decoding functions.
 */

#include <algorithm>
#include <stdexcept>
#include <sstream>
#include <utility>
#include <boost/algorithm/string.hpp>
#if defined ( _WIN32 )
#include <windows.h>
//#include <cryptopp/dll.h>     // msvc-shared only
#endif  // defined ( _WIN32 )
#include <cryptopp/aes.h>
#include <cryptopp/ccm.h>
#include <cryptopp/cryptlib.h>
#include <cryptopp/filters.h>
#include <cryptopp/hex.h>
#include <cryptopp/sha.h>
#include <b64/decode.h>
#include <b64/encode.h>
#include "Crypto.hpp"
#include "make_unique.hpp"

#define     UNKNOWN_ERROR           "Unknown error!"

using namespace std;
using namespace boost;
using namespace CryptoPP;
using namespace CoreLib;

struct Crypto::Impl
{
public:
    Crypto::Byte *Key;
    std::size_t KeyLen;
    Crypto::Byte *IV;
    std::size_t IVLen;

public:
    Impl();
    ~Impl();

};

bool Crypto::Encrypt(const std::string &plainText, std::string &out_encodedText,
                     const Byte *key, std::size_t keyLen, const Byte *iv, std::size_t ivLen)
{
    string err;
    return Crypto::Encrypt(plainText, out_encodedText, err, key, keyLen, iv, ivLen);
}

bool Crypto::Encrypt(const std::string &plainText, std::string &out_encodedText,
                     std::string &out_error,
                     const Byte *key, std::size_t keyLen, const Byte *iv, std::size_t ivLen)
{
    try {
        CBC_Mode<AES>::Encryption enc;
        enc.SetKeyWithIV(key, keyLen, iv, ivLen);

        string cipher;
        StringSource(plainText, true,
                     new StreamTransformationFilter(enc, new StringSink(cipher)));

        string encoded;
        StringSource(cipher, true, new HexEncoder(new StringSink(encoded)));
        out_encodedText.assign(std::move(encoded));

        return true;
    }

    catch (const CryptoPP::Exception &ex) {
        out_error.assign(ex.what());
    }

    catch (const std::exception &ex) {
        out_error.assign(ex.what());
    }

    catch (...) {
        out_error.assign(UNKNOWN_ERROR);
    }

    return false;
}

bool Crypto::Decrypt(const std::string &cipherText, std::string &out_recoveredText,
                     const Byte *key, std::size_t keyLen, const Byte *iv, std::size_t ivLen)
{
    string err;
    return Crypto::Decrypt(cipherText, out_recoveredText, err, key, keyLen, iv, ivLen);
}

bool Crypto::Decrypt(const std::string &cipherText, std::string &out_recoveredText,
                     std::string &out_error,
                     const Byte *key, std::size_t keyLen, const Byte *iv, std::size_t ivLen)
{
    try {
        CBC_Mode<AES>::Decryption dec;
        dec.SetKeyWithIV(key, keyLen, iv, ivLen);

        string cipher;
        StringSource(cipherText, true, new HexDecoder(new StringSink(cipher)));

        string decoded;
        StringSource(cipher, true,
                     new StreamTransformationFilter(dec, new StringSink(decoded)));
        out_recoveredText.assign(std::move(decoded));

        return true;
    }

    catch (const CryptoPP::Exception &ex) {
        out_error.assign(ex.what());
    }

    catch (const std::exception &ex) {
        out_error.assign(ex.what());
    }

    catch (...) {
        out_error.assign(UNKNOWN_ERROR);
    }

    return false;
}

bool Crypto::Hash(const std::string &text, std::string &out_digest)
{
    string err;
    return Hash(text, out_digest, err);
}

bool Crypto::Hash(const std::string &text, std::string &out_digest,
                  std::string &out_error)
{
    try {
        SHA512 hash;

        string digest;
        StringSource(text, true,
                     new HashFilter(hash, new HexEncoder(new StringSink(digest))));
        out_digest.assign(std::move(digest));

        return true;
    }

    catch (const CryptoPP::Exception &ex) {
        out_error.assign(ex.what());
    }

    catch (const std::exception &ex) {
        out_error.assign(ex.what());
    }

    catch (...) {
        out_error.assign(UNKNOWN_ERROR);
    }

    return false;
}

int Crypto::Base64Decode(char value)
{
    base64::decoder decoder;
    return decoder.decode(value);
}

int Crypto::Base64Decode(const char *code, const int length, char *out_plainText)
{
    base64::decoder decoder;
    return decoder.decode(code, length, out_plainText);
}

void Crypto::Base64Decode(std::istream &inputStream, std::ostream &outputStream)
{
    base64::decoder decoder;
    decoder.decode(inputStream, outputStream);
}

int Crypto::Base64Encode(char value)
{
    base64::encoder encoder;
    return encoder.encode(value);
}

int Crypto::Base64Encode(const char *code, const int length, char *out_plainText)
{
    base64::encoder encoder;
    return encoder.encode(code, length, out_plainText);
}

int Crypto::Base64EncodeBlockEnd(char *out_plainText)
{
    base64::encoder encoder;
    return encoder.encode_end(out_plainText);
}

void Crypto::Base64Encode(std::istream &inputStream, std::ostream &outputStream)
{
    base64::encoder encoder;
    encoder.encode(inputStream, outputStream);
}

Crypto::Crypto(const Byte *key, std::size_t keyLen, const Byte *iv, std::size_t ivLen) :
    m_pimpl(make_unique<Crypto::Impl>())
{
    m_pimpl->Key = new Byte[keyLen];
    m_pimpl->IV = new Byte[ivLen];

    std::copy(key, key + keyLen, m_pimpl->Key);
    std::copy(iv, iv + ivLen, m_pimpl->IV);

    m_pimpl->KeyLen = keyLen;
    m_pimpl->IVLen = ivLen;
}

Crypto::~Crypto()
{

}

bool Crypto::Encrypt(const std::string &plainText, std::string &out_encodedText)
{
    return Crypto::Encrypt(plainText, out_encodedText, m_pimpl->Key, m_pimpl->KeyLen, m_pimpl->IV, m_pimpl->IVLen);
}

bool Crypto::Encrypt(const std::string &plainText, std::string &out_encodedText,
                     std::string &out_error)
{
    return Crypto::Encrypt(plainText, out_encodedText, out_error, m_pimpl->Key, m_pimpl->KeyLen, m_pimpl->IV, m_pimpl->IVLen);
}

bool Crypto::Decrypt(const std::string &cipherText, std::string &out_recoveredText)
{
    return Crypto::Decrypt(cipherText, out_recoveredText, m_pimpl->Key, m_pimpl->KeyLen, m_pimpl->IV, m_pimpl->IVLen);
}

bool Crypto::Decrypt(const std::string &cipherText, std::string &out_recoveredText,
                     std::string &out_error)
{
    return Crypto::Encrypt(cipherText, out_recoveredText, out_error, m_pimpl->Key, m_pimpl->KeyLen, m_pimpl->IV, m_pimpl->IVLen);
}

std::string Crypto::ByteArrayToString(const unsigned char *array, size_t length)
{
    ostringstream oss;

    for (size_t i = 0; i < length; ++i) {
        oss << array[i];
    }

    return oss.str();
}

std::wstring Crypto::WCharArrayToString(const wchar_t *array, size_t length)
{
    wostringstream woss;

    for (size_t i = 0; i < length; ++i) {
        woss << array[i];
    }

    return woss.str();
}

std::string Crypto::HexStringToString(const std::string &hexString)
{
    vector<string> bytes;
    algorithm::iter_split(bytes, hexString, algorithm::first_finder(":"));

    ostringstream oss;

    for (vector<string>::iterator it = bytes.begin(); it != bytes.end(); ++it) {
        oss << (char)strtol(it->c_str(), NULL, 16);
    }

    return oss.str();
}

std::wstring Crypto::HexStringToWString(const std::wstring &hexString)
{
    vector<wstring> bytes;
    algorithm::iter_split(bytes, hexString, algorithm::first_finder(":"));

    wostringstream woss;

    for (vector<wstring>::iterator it = bytes.begin(); it != bytes.end(); ++it) {
        woss << (wchar_t)wcstol(it->c_str(), NULL, 16);
    }

    return woss.str();
}

Crypto::Impl::Impl()
{

}

Crypto::Impl::~Impl()
{
    delete IV;
    delete Key;
}