#ifndef FSTCLASSIFIERKNN_H
#define FSTCLASSIFIERKNN_H

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

   \file    classifier_knn.hpp
   \brief   Implements k-Nearest Neighbor classifier
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.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 <cmath>
#include <list>
#include "classifier.hpp"
#include "error.hpp"
#include "global.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 {

//! Implements k-Nearest Neighbor classifier
template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
class Classifier_kNN : public Classifier<RETURNTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Classifier<RETURNTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	typedef typename DATAACCESSOR::PPattern PPattern;
	Classifier_kNN() {set_k(1); _nns_max_size=1; notify("Classifier_kNN constructor.");}
	virtual ~Classifier_kNN() {notify("Classifier_kNN destructor.");}

	void set_k(const DIMTYPE k) {assert(k>0); _k=k; _nns_max_size=_k;} // NOTE: _nns_max_size may be set larger to help avoiding ties
	DIMTYPE get_k() const {return _k;}

	virtual bool classify(DIMTYPE &cls, const PPattern &pattern);  // classifies pattern, returns the respective class index
	virtual bool train(const PDataAccessor da, const PSubset sub); // with kNN there is actually no training. This just stores pointer to training data to be later accessed from test()
	virtual bool test(RETURNTYPE &result, const PDataAccessor da); // estimates accuracy using designated test data

	Classifier_kNN* clone() const;
	Classifier_kNN* sharing_clone() const {throw fst_error("Classifier_kNN::sharing_clone() not supported, use Classifier_kNN::clone() instead.");}
	Classifier_kNN* stateless_clone() const {throw fst_error("Classifier_kNN::stateless_clone() not supported, use Classifier_kNN::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const;
private:
	Classifier_kNN(const Classifier_kNN& cknn); // copy-constructor
protected:
	// NOTE: for given k it is better to keep ((k-1)*NoOfClasses+1) nearest neighbours to prevent ties (provided enough neighbours exist)
	// NOTE: equal neighbour distances to different classes are not handled extra

	//! Holds information on distance and data-class membership of neighbors processed in Classifier_kNN
	class Neighbour{
	public:
		Neighbour() {}
		Neighbour(const RETURNTYPE value, const DIMTYPE cls) {_value=value; _cls=cls;}
		Neighbour(const Neighbour& nei) {_value=nei._value; _cls=nei._cls;}	//!< copy constructor
		RETURNTYPE _value;
		DIMTYPE _cls;
	};
	DIMTYPE _k;
	DIMTYPE _k_enough;
	DIMTYPE _nns_max_size;
	void sort_in(const RETURNTYPE value, const DIMTYPE cls);
	DIMTYPE get_most_freq_cls();
	typename std::list<Neighbour>::iterator iter;
	std::list<Neighbour> _nns; // implemented descending ... the closest neighbour is the last
	std::vector<DIMTYPE> cls_freqs;

	boost::scoped_ptr<DISTANCE> _distance;
	PDataAccessor _da_train; // with kNN there is actually no training. This is meant to store pointer to training data to be later accessed from test()
};

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::Classifier_kNN(const Classifier_kNN& cknn) :
	_k(cknn._k),
	_k_enough(cknn._k_enough),
	_nns_max_size(cknn._nns_max_size),
	_nns(cknn._nns),
	cls_freqs(cknn.cls_freqs)
{
	notify("Classifier_kNN constructor.");
	if(cknn._distance) _distance.reset(cknn._distance->clone());
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>* Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::clone() const
{
	Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE> *clone=new Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
bool Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::train(const PDataAccessor da, const PSubset sub)
{
	assert(da);
	assert(sub);
	if(!_distance || _distance->get_n()<sub->get_n_raw()) _distance.reset(new DISTANCE(sub->get_n_raw()));
	_distance->narrow_to(sub);
	_da_train=da;
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
bool Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::classify(DIMTYPE &cls, const PPattern &pattern)
{
	// NOTE: uses da_train_loop=0
	// NOTE: da and _da_train may point to the same instance -> be careful with setClass() etc.
	assert(_da_train);
	assert(_distance);
	assert(_distance->get_n()==_da_train->getNoOfFeatures());
	assert(_distance->get_d()>0);
	typename DATAACCESSOR::PPattern p2;
	IDXTYPE s2,i2;
	bool b;
	DIMTYPE _feats=_da_train->getNoOfFeatures();
	RETURNTYPE val;

	const DIMTYPE da_train_loop=0; // to avoid mixup of get*Block() loops of different types be careful when calling classify() from a different loop

	cls_freqs.resize(_da_train->getNoOfClasses());
	_nns_max_size=(_k-1)*_da_train->getNoOfClasses()+1; // to help avoiding ties
	_k_enough=DIMTYPE(floor(0.5*(RETURNTYPE)_k)+1);

	_nns.clear();
	for(DIMTYPE c_train=0;c_train<_da_train->getNoOfClasses();c_train++)
	{
		_da_train->setClass(c_train);
		for(b=_da_train->getFirstBlock(TRAIN,p2,s2,da_train_loop);b==true;b=_da_train->getNextBlock(TRAIN,p2,s2,da_train_loop)) for(i2=0;i2<s2;i2++)
		{
			// p2[i2*_feats] is the beginning of the current pattern
			val=_distance->distance(pattern,&p2[i2*_feats]);
			sort_in(val,c_train);
		}
	}
	cls=get_most_freq_cls();

	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
bool Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::test(RETURNTYPE &result, const PDataAccessor da)
{
	// NOTE: da and _da_train may point to the same instance -> be careful with setClass() etc.
	assert(da);
	assert(_da_train);
	assert(_distance);
	assert(da->getNoOfFeatures()==_da_train->getNoOfFeatures());
	assert(_distance->get_n()==_da_train->getNoOfFeatures());
	typename DATAACCESSOR::PPattern p1;
	IDXTYPE s1,i1;
	bool b;
	IDXTYPE count=0, correct=0;
	DIMTYPE _feats=da->getNoOfFeatures();
	DIMTYPE clstmp;

	const DIMTYPE da_test_loop=1; // to avoid mixup of get*Block() loops of different types (must not! be 0, see classify())

	cls_freqs.resize(_da_train->getNoOfClasses());
	_nns_max_size=(_k-1)*_da_train->getNoOfClasses()+1; // to help avoiding ties
	_k_enough=DIMTYPE(floor(0.5*(RETURNTYPE)_k)+1);

	for(DIMTYPE c_test=0;c_test<da->getNoOfClasses();c_test++)
	{
		da->setClass(c_test);
		for(b=da->getFirstBlock(TEST,p1,s1,da_test_loop);b==true;b=da->getNextBlock(TEST,p1,s1,da_test_loop)) {
			for(i1=0;i1<s1;i1++)
			{
				if(!classify(clstmp,&p1[i1*_feats])) return false; // \note classify() internally uses block loop index 0
				if(clstmp==c_test) ++correct;
				++count;
			}
			da->setClass(c_test); // necessary for the case of da pointing to the same object as _da_train
		}
	}
	if(count==0) return false;
	result = (RETURNTYPE)correct/(RETURNTYPE)count;
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
void Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::sort_in(const RETURNTYPE value, const DIMTYPE cls)
{
	iter=_nns.begin();
	while(iter!=_nns.end() && value < (*iter)._value) iter++;
	if(_nns.size()<_nns_max_size || iter!=_nns.begin()) {
		Neighbour tmp(value,cls);
		_nns.insert(iter,tmp);
		// NOTE: ? consider: if(as below && first value!= second value) _nns.pop_front() .. to prevent loosing tie info
		if(_nns.size()>_nns_max_size) _nns.pop_front();
	}
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
DIMTYPE Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::get_most_freq_cls()
{ // NOTE: destructs contents of _nns !
	assert(_da_train);
	DIMTYPE i,c,c_max,max=0;
	for(i=0;i<_da_train->getNoOfClasses();i++) cls_freqs[i]=0;
	while(_nns.size()>0) {
		c=(_nns.back()._cls);
		if(++cls_freqs[c]==_k_enough) return c;
		if(cls_freqs[c]>max) {max=cls_freqs[c]; c_max=c;}
		_nns.pop_back();
	}
	if(max>0) return c_max;
	else return _da_train->getNoOfClasses(); // wrong class
}

template<class RETURNTYPE, typename DATATYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR, class DISTANCE>
std::ostream& Classifier_kNN<RETURNTYPE,DATATYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR,DISTANCE>::print(std::ostream& os) const
{
	os << "Clasifier_kNN(k=" << _k << ")";
	if(_distance) os << *_distance;
	return os;
}

} // namespace
#endif // FSTCLASSIFIERKNN_H ///:~


/* boost test_random_number_generator.cpp
 *
 * Copyright Jens Maurer 2000
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id: test_random_number_generator.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 */

#include <boost/random/random_number_generator.hpp>
#include <boost/random/mersenne_twister.hpp>

#include <algorithm>
#include <vector>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_random_shuffle)
{
    boost::mt19937 engine(1234);
    boost::random::random_number_generator<boost::mt19937> generator(engine);

    std::vector<int> testVec;

    for (int i = 0; i < 200; ++i) {
        testVec.push_back(i);
    }

    std::random_shuffle(testVec.begin(), testVec.end(), generator);
}


//
// Created by qfeng on 2019-01-04.
//

#include "feature/bucketize_op_ctx.h"
#include "feature/bucketize_op.h"
#include "resource/resource_manager.h"
#include <google/protobuf/arena.h>
#include <absl/strings/str_split.h>
#include <string>
#include <boost/algorithm/string.hpp>

using namespace boost::algorithm;

namespace algocomp {
    vector<float> BucketizeOpCtx::_ten_equal_width = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1};

    // -1.645 ~ 1.645 wrt lower_p=0.95
    vector<float> BucketizeOpCtx::_ten_equal_width_zscore = {-0.165, -0.132, -0.099, -0.066, -0.033, 0., 0.033, 0.066,
                                                             0.099, 0.132, 0.165};

    bool BucketizeOpCtx::parse_procs_and_params(const string &proc_desc, const string &param_desc) {
        try {
            // Check the lengths of inputs (necessary inputs must not be empty)
            if (proc_desc == "" && param_desc == "") {
                _logger->warn(
                        "Empty procs and params for bucketize feature operation context (in_gname={}, in_fname={}).",
                        _in_gname, _in_fname);
                return false;
            }

            // Parse processes together with their parameters
            std::vector<std::string> proc_splits = absl::StrSplit(proc_desc, ';');
            std::vector<std::string> param_splits = absl::StrSplit(param_desc, ';');

            // The processes and parameters are corresponding one by one. The belows are to check the correctness
            if (proc_splits.size() != param_splits.size()) {
                _logger->error(
                        "Parsed inconsistent sizes of procs {} and params {} (in_gname={}, in_fname={}).",
                        proc_desc, param_desc, _in_gname, _in_fname);
                return false;
            }

            for (int i = 0; i < proc_splits.size(); ++i) {
                auto proc_name = trim_copy(proc_splits[i]);

                if (proc_name == "customized") { // Parse process of customized boundaries
                    _boundaries.clear();
                    std::vector<std::string> c_splits = absl::StrSplit(param_splits[i], ',');
                    for (auto &s: c_splits) {
                        _boundaries.emplace_back(std::move(std::stof(s)));
                    }
                } else if (proc_name == "predefined") { // Parse process of predefined boundaries
                    if (trim_copy(param_splits[i]) == "ten-equal-width") {
                        _boundaries = BucketizeOpCtx::_ten_equal_width;
                    } else if (trim_copy(param_splits[i]) == "ten-equal-width-zscore") {
                        _boundaries = BucketizeOpCtx::_ten_equal_width_zscore;
                    } else {
                        _logger->error("Parsed unknown bucketize param {} (in_gname={}, in_fname={}).",
                                       param_splits[i],
                                       _in_gname, _in_fname);
                        return false;
                    }
                } else {
                    _logger->error("Parsed unknown bucketize proc {} (in_gname={}, in_fname={}).",
                                   proc_splits[i],
                                   _in_gname, _in_fname);
                    return false;
                }
            }

            return true;

        } catch (const std::exception &ex) {
            _logger->error("Parsed wrong bucketize procs {} and params {} (in_gname={}, in_fname={}).",
                           proc_desc,
                           param_desc, _in_gname, _in_fname);
        }

        return false;
    }

    const vector<float> BucketizeOpCtx::boundaries() const {
        return _boundaries;
    }

    optional<pair<string, DiscFeatGrp>>
    BucketizeOpCtx::apply(const vector<google::protobuf::Map<string, algocomp::ContFeatGrp>> &inp_feats,
                          google::protobuf::Arena &arena) const {
        try {
            DiscFeatGrp *outp_feats = google::protobuf::Arena::CreateMessage<DiscFeatGrp>(&arena);

            for (auto &inp_feats_: inp_feats) {
                // Match input feature
                auto g_found = inp_feats_.find(_in_gname);
                if (g_found != inp_feats_.end()) {
                    auto &feats = (*g_found).second.feats();

                    if (this->op_scope() == OP_SCOPE::GNAME) {
                        for (auto &feat: feats) {
                            // Apply bucketizing transform on every feature of matched feature group name
                            auto res = this->_op->apply(feat.second, *this, arena);
                            if (res.has_value()) {
                                auto &res_feat = res.value();
                                (*outp_feats->mutable_feats())[res_feat.fname()] = res_feat;
                            } else {
                                _logger->warn("Bucketize fail on feature {} - {}", _in_gname, feat.first);
                            }
                        }
                    } else if (this->op_scope() == OP_SCOPE::FNAME) {
                        auto f_found = feats.find(_in_fname);
                        if (f_found != feats.end()) {
                            // Apply bucketizing transform on matched feature
                            auto &feat = (*f_found).second;
                            auto res = this->_op->apply(feat, *this, arena);

                            if (res.has_value()) {
                                auto &res_feat = res.value();
                                (*outp_feats->mutable_feats())[res_feat.fname()] = res_feat;
                            } else {
                                _logger->warn("Bucketize fail on feature {} - {}", _in_gname, feat.fname());
                            }
                        }
                    }
                }
            }

            if (outp_feats->feats_size() > 0) {
                auto outp_gname = outp_feats->feats().begin()->second.gname();
                return make_pair(outp_gname, *outp_feats);
            } else {
                return std::nullopt;
            }
        } catch (const exception &ex) {
            _logger->error("Bucketize fail on features: {}.", ex.what());
        }

        return std::nullopt;
    }
}


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

// Copyright (c) 2010 Alfredo Correa
// Copyright (c) 2010-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/adapted/boost_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/std_array.hpp>
#include <boost/geometry/geometries/point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_STD_ARRAY_CS(cs::cartesian)

int test_main(int, char* [])
{
    bg::model::point<double, 3, bg::cs::cartesian> p1(1,2,3);
    double p2[3] = {4,5,6};
    boost::tuple<double, double, double> p3(7,8,9);
    boost::array<double, 3> p4 = {{10,11,12}};
    std::array<double, 3> p5 = {{13,14,15}};

    std::clog << bg::distance(p1, p2) << std::endl;
    std::clog << bg::distance(p2, p3) << std::endl;
    std::clog << bg::distance(p3, p4) << std::endl;
    std::clog << bg::distance(p4, p5) << std::endl;

    return 0;
}


#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/CompressedImage.h>

#include "cloud_to_spi.h"
#include "spi_utility.h"

namespace {
std::vector<std::string> split(const std::string& str, const std::string& delim)
{
    std::vector<std::string> tokens;
    size_t prev = 0, pos = 0;
    do {
        pos = str.find(delim, prev);
        if (pos == std::string::npos)
            pos = str.length();
        std::string token = str.substr(prev, pos - prev);
        if (!token.empty())
            tokens.push_back(token);
        prev = pos + delim.length();
    } while (pos < str.length() && prev < str.length());

    return tokens;
}
}  // namespace

CloudToSpi::CloudToSpi(const ros::NodeHandle& node_handler) : nh_(node_handler)
{
    tf_buffer_.reset(new tf2_ros::Buffer(ros::Duration(3600)));

    nh_.param<std::string>("world_frame", world_frame_, "world");
    nh_.param<std::string>("lidar_frame", lidar_frame_, "velodyne");
    nh_.param<int>("num_accumulated_clouds", num_accumulated_clouds_, 600);
    nh_.param<int>("inter_cloud_diff", inter_cloud_diff_, 200);
    nh_.param<float>("max_z", max_z_, 10.f);
    nh_.param<float>("min_z", min_z_, 0.f);
    nh_.param<float>("scale", scale_, 100.f);
    nh_.param<float>("meters_per_pixel", meters_per_pixel_, 0.1f);
    nh_.param<double>("x_offset", x_offset_, -2861852);
    nh_.param<double>("y_offset", y_offset_, 4651685);
    nh_.param<double>("z_offset", z_offset_, 3283262);

    nh_.param<std::string>("lidar_trajectory_file",
                           lidar_trajectory_file_,
                           "/media/li/lavie/dataset/20201023114733_Paul_Locate.bag_lidar_trajectory.txt");

    ROS_ASSERT(num_accumulated_clouds_ >= inter_cloud_diff_);

    ptcloud_subscriber_ = nh_.subscribe(kPointCloudTopic, 100, &CloudToSpi::pointcloud_callback, this);
    spi_publisher_ = nh_.advertise<sensor_msgs::CompressedImage>(kSpiTopic, 2);

    ROS_INFO_STREAM("Start loading tf_buffer ...");
    load_tf_buffer();
    ROS_INFO_STREAM("Loading tf_buffer finished.");
    // play_spi();
}

void CloudToSpi::pointcloud_callback(const sensor_msgs::PointCloud2ConstPtr& ptcloud_msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*ptcloud_msg, *point_cloud);
    try {
        auto stamped_transform = tf_buffer_->lookupTransform(world_frame_, lidar_frame_, ptcloud_msg->header.stamp);
        // LOG(INFO) << stamped_transform;
        // Eigen::Isometry3d lidar_to_world = Eigen::Isometry3d::Identity();
        Eigen::Quaterniond quaternion(stamped_transform.transform.rotation.w,
                                      stamped_transform.transform.rotation.x,
                                      stamped_transform.transform.rotation.y,
                                      stamped_transform.transform.rotation.z);
        Eigen::Matrix4d lidar_to_world = Eigen::Matrix4d::Identity();
        lidar_to_world.block(0, 0, 3, 3) = quaternion.matrix();
        lidar_to_world.block(0, 3, 3, 1) << stamped_transform.transform.translation.x,
                stamped_transform.transform.translation.y, stamped_transform.transform.translation.z;
        // ROS_INFO_STREAM("lidar_to_world: \n" << lidar_to_world);

        // 1. transform cloud to world frame
        // filter_cloud_by_z_value(pcl_point_cloud, FLAGS_filer_min_z, FLAGS_filer_max_z);
        filter_cloud_by_z_value(*point_cloud, min_z_, max_z_);
        pcl::transformPointCloud(*point_cloud, *point_cloud, lidar_to_world);

        cloud_queue_.push_back(point_cloud);
        poses_lidar_to_world_.push_back(lidar_to_world);

        // ROS_INFO_STREAM("point_cloud size: " << point_cloud->size());

        if (cloud_queue_.size() >= num_accumulated_clouds_ + inter_cloud_diff_) {
            ROS_ASSERT(cloud_queue_.size() == poses_lidar_to_world_.size());
            int ref_index = num_accumulated_clouds_ / 2;

            Eigen::Matrix4d lidar_ref_to_world = poses_lidar_to_world_[ref_index];
            pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_accumulated(new pcl::PointCloud<pcl::PointXYZ>());
            for (int i = 0; i < num_accumulated_clouds_; ++i) {
                *point_cloud_accumulated += *(cloud_queue_[i]);
            }

            pcl::transformPointCloud(*point_cloud_accumulated,
                                     *point_cloud_accumulated,
                                     lidar_ref_to_world.inverse().cast<float>());

            static int submap_id = 0;
            cv::Mat spi_image;
            // spi_image =
            //         cv::imread("/media/li/lavie/dataset/birdview_dataset/08/submap_" + std::to_string(submap_id++) +
            //         ".png");
            // ROS_INFO_STREAM("point_cloud_accumulated len: " << point_cloud_accumulated->size());
            cloud_to_birdview_image(*point_cloud_accumulated, spi_image, min_z_, max_z_, scale_, meters_per_pixel_);
            spi_image.convertTo(spi_image, CV_8U, 255);
            cv::imshow("spi", spi_image);
            cv::waitKey(1);

            sensor_msgs::CompressedImage img_msg;
            img_msg.header = ptcloud_msg->header;
            img_msg.format = "png";
            cv::imencode(".png", spi_image, img_msg.data);

            spi_publisher_.publish(img_msg);
            ROS_INFO_STREAM("Published spi image.");

            for (int i = 0; i < inter_cloud_diff_; ++i) {
                cloud_queue_.pop_front();
                poses_lidar_to_world_.pop_front();
            }
        }
    } catch (const tf2::TransformException& ex) {
        ROS_INFO(ex.what());
    }
}

void CloudToSpi::load_tf_buffer()
{
    std::ifstream lidar_trajectory_file(lidar_trajectory_file_);
    std::string line;
    int seq = 0;
    while (std::getline(lidar_trajectory_file, line)) {
        auto splitted = split(line, ",");
        geometry_msgs::TransformStamped transform_stamped_msg;
        transform_stamped_msg.header.frame_id = world_frame_;
        transform_stamped_msg.child_frame_id = lidar_frame_;
        transform_stamped_msg.header.seq = seq++;
        // transform_stamped_msg.header.stamp = ros::Time::fromSec(atof(splitted[0].c_str()));
        // transform_stamped_msg.transform.rotation.w = pose_stamped_msg.pose.orientation;
        transform_stamped_msg.header.stamp.fromSec(atof(splitted[0].c_str()));
        transform_stamped_msg.transform.translation.x = atof(splitted[1].c_str()) - x_offset_;
        transform_stamped_msg.transform.translation.y = atof(splitted[2].c_str()) - y_offset_;
        transform_stamped_msg.transform.translation.z = atof(splitted[3].c_str()) - z_offset_;
        transform_stamped_msg.transform.rotation.w = atof(splitted[4].c_str());
        transform_stamped_msg.transform.rotation.x = atof(splitted[5].c_str());
        transform_stamped_msg.transform.rotation.y = atof(splitted[6].c_str());
        transform_stamped_msg.transform.rotation.z = atof(splitted[7].c_str());
        tf_buffer_->setTransform(transform_stamped_msg, "li", false);
    }
}

void CloudToSpi::play_spi()
{
    static int submap_id = 0;
    ros::Rate rate(2.5);
    while (ros::ok()) {
        cv::Mat spi_image;
        const std::string spi_filename =
                "/media/li/lavie/dataset/birdview_dataset/08/submap_" + std::to_string(submap_id++) + ".png";
        spi_image = cv::imread(spi_filename, cv::IMREAD_GRAYSCALE);
        ROS_INFO_STREAM(spi_image.channels());
        ROS_INFO_STREAM(spi_image.size);
        cv::imshow("spi raw", spi_image);
        cv::waitKey(1);
        // cloud_to_birdview_image(*point_cloud, spi_image);

        sensor_msgs::CompressedImage img_msg;
        img_msg.format = "png";
        cv::imencode(".png", spi_image, img_msg.data);

        spi_publisher_.publish(img_msg);
        ROS_INFO_STREAM("Played " << spi_filename);
        rate.sleep();
    }
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    semiMajorAixscopy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include "Tudat/SimulationSetup/tudatSimulationHeader.h"
#include "Tudat/Astrodynamics/Aerodynamics/UnitTests/applicationOutput.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{

namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_aerodynamic_acceleration_force_moment_models )

BOOST_AUTO_TEST_CASE( testTabulatedDragCoefficient )
{
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            USING STATEMENTS              //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    using namespace tudat;
    using namespace simulation_setup;
    using namespace propagators;
    using namespace numerical_integrators;
    using namespace orbital_element_conversions;
    using namespace basic_mathematics;
    using namespace basic_astrodynamics;
    using namespace gravitation;
    using namespace numerical_integrators;
    using namespace interpolators;
    using namespace input_output;

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////     CREATE ENVIRONMENT AND VEHICLE       //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation time settings.
    const double simulationStartEpoch = 0.0;
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    // Define body settings for simulation.
    std::vector< std::string > bodiesToCreate;
    bodiesToCreate.push_back( "Sun" );
    bodiesToCreate.push_back( "Earth" );
    bodiesToCreate.push_back( "Moon" );

    // Create body objects.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings =
            getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 );

    for( unsigned int i = 0; i < bodiesToCreate.size( ); i++ )
    {
        bodySettings[ bodiesToCreate.at( i ) ]->ephemerisSettings->resetFrameOrientation( "J2000" );
        bodySettings[ bodiesToCreate.at( i ) ]->rotationModelSettings->resetOriginalFrame( "J2000" );
    }

    // EARTH
    bodySettings[ "Earth" ]->gravityFieldSettings = std::make_shared< GravityFieldSettings >( central_spice );
    bodySettings[ "Earth" ]->atmosphereSettings = std::make_shared< AtmosphereSettings >( nrlmsise00 );

    // MOON
    bodySettings[ "Moon" ]->gravityFieldSettings = std::make_shared< GravityFieldSettings >( central_spice );

    NamedBodyMap bodyMap = createBodies( bodySettings );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE VEHICLE            /////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Create spacecraft object.
    bodyMap[ "Vehicle" ] = std::make_shared< simulation_setup::Body >( );
    bodyMap[ "Vehicle" ]->setConstantBodyMass( 400.0 );
    double referenceArea = 10.0;

    std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings;

    // Read CD as semiMajorAixsfunction of altitude
    Eigen::MatrixXd aerodynamicsDataFromFile = readMatrixFromFile(
                tudat_applications::getOutputPath( ) + "tabulatedDragCoefficient.txt" );
    Eigen::VectorXd altitudeInKm = aerodynamicsDataFromFile.col(0);
    std::vector< double > altitudes;
    for ( int i = 0; i < altitudeInKm.rows( ); i++ )
    {
        altitudes.push_back( altitudeInKm( i ) * 1.0E3 );
    }
    Eigen::VectorXd dragCoefficientsFromFile = aerodynamicsDataFromFile.col( 1 );
    std::vector< double > dragCoefficients(
                dragCoefficientsFromFile.data( ), dragCoefficientsFromFile.data( ) + dragCoefficientsFromFile.size( ) );
    std::vector< Eigen::Vector3d > aerodynamicCoefficients;
    for ( unsigned int i = 0; i < dragCoefficients.size( ); i++ )
    {
        aerodynamicCoefficients.push_back( dragCoefficients[ i ] * Eigen::Vector3d::UnitX( ) );
    }

    // Create interpolator
    std::shared_ptr< InterpolatorSettings > interpolatorSettings =
            std::make_shared< InterpolatorSettings >( InterpolatorTypes::linear_interpolator );

    // Tabulated aerodynamic settings
    aerodynamicCoefficientSettings = std::make_shared< TabulatedAerodynamicCoefficientSettings< 1 > >(
                altitudes, aerodynamicCoefficients, referenceArea,
                aerodynamics::altitude_dependent, 1, 1, interpolatorSettings );

    // Aerodynamics interface
    bodyMap[ "Vehicle" ]->setAerodynamicCoefficientInterface(
                createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Vehicle" ) );

    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////            CREATE ACCELERATIONS          //////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;

    accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
                                                   basic_astrodynamics::central_gravity ) );
    accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
                                                    basic_astrodynamics::central_gravity ) );
    accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::aerodynamic ) );

    accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;

    bodiesToPropagate.push_back( "Vehicle" );
    centralBodies.push_back( "Earth" );

    basic_astrodynamics::AccelerationMap accelerationModelMap =
            createAccelerationModelsMap( bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             CREATE PROPAGATION SETTINGS            ////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    double meanEarthRadius = 6371.0E3;
    double perigeeAltitude = 150.0E3;
    double apogeeAltitude = 35780.0E3;
    double semiMajorAixs= (apogeeAltitude + perigeeAltitude) / 2.0 + meanEarthRadius;
    double eccentricity = (apogeeAltitude - perigeeAltitude)/(apogeeAltitude + perigeeAltitude + 2*meanEarthRadius);

    // Set Keplerian elements for Vehicle.
    Eigen::Vector6d vehicleInitialStateInKeplerianElements = Eigen::Vector6d::Zero( );
    vehicleInitialStateInKeplerianElements( semiMajorAxisIndex ) = semiMajorAixs;
    vehicleInitialStateInKeplerianElements( eccentricityIndex ) = eccentricity;
    vehicleInitialStateInKeplerianElements( inclinationIndex ) = mathematical_constants::PI / 180.0 * 23.4;

    double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    const Eigen::Vector6d vehicleInitialState = convertKeplerianToCartesianElements(
                vehicleInitialStateInKeplerianElements, earthGravitationalParameter );

    // Hybrid termination conditions
    std::vector< std::shared_ptr< propagators::PropagationTerminationSettings > > constituentSettings;

    // Time limit
    constituentSettings.push_back( std::make_shared< propagators::PropagationTimeTerminationSettings >(
                                       simulationEndEpoch ) );

    // Altitude limit
    std::shared_ptr< PropagationTerminationSettings > altitudeTerminationSettings =
            std::make_shared< propagators::PropagationDependentVariableTerminationSettings >(
                std::make_shared< propagators::SingleDependentVariableSaveSettings >(
                    propagators::altitude_dependent_variable, "Vehicle" ), 100.0E3, 1 );
    constituentSettings.push_back( altitudeTerminationSettings );

    // Stop if ANY of the two is met
    std::shared_ptr< PropagationTerminationSettings > terminationSettings =
            std::make_shared< propagators::PropagationHybridTerminationSettings >( constituentSettings, 1 );

    // Save dependent variables
    std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesToSave;
    dependentVariablesToSave.push_back( std::make_shared< SingleDependentVariableSaveSettings >(
                                            altitude_dependent_variable, "Vehicle" ) );
    dependentVariablesToSave.push_back( std::make_shared< SingleDependentVariableSaveSettings >(
                                            aerodynamic_moment_coefficients_dependent_variable, "Vehicle" ) );

    std::shared_ptr< DependentVariableSaveSettings > dependentVariableSaveSettings =
            std::make_shared< DependentVariableSaveSettings >( dependentVariablesToSave, 0 ) ;

    // Translational propagator settings
    std::shared_ptr< TranslationalStatePropagatorSettings< double > > translationalPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > > (
                centralBodies, accelerationModelMap, bodiesToPropagate, vehicleInitialState, terminationSettings );

    std::shared_ptr< IntegratorSettings< > > integratorSettings = std::make_shared< IntegratorSettings< > > (
                rungeKutta4, simulationStartEpoch, 300.0 );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////             PROPAGATE ORBIT            ////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodyMap, integratorSettings, translationalPropagatorSettings, true, false, false );
    std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > dependentVariableOutput =
            dynamicsSimulator.getDependentVariableHistory( );

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    ///////////////////////       CHECK DRAG COEFFICIENTS         ////////////////////////////////////////////////////////
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    // Retrieve altitudes and drag coefficients from propagation results

    Eigen::Vector3d currentCoefficients;
    double currentAltitude;
    double computedDragCoefficient;
    LinearInterpolatorDoublePointer dragCoefficientInterpolator =
            std::make_shared<LinearInterpolatorDouble>( LinearInterpolatorDouble( altitudes, dragCoefficients ) );

    for ( std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > >::iterator outputIterator =
          dependentVariableOutput.begin( ); outputIterator != dependentVariableOutput.end( ); ++outputIterator )
    {
        currentAltitude = outputIterator->second( 0 );
        computedDragCoefficient = dragCoefficientInterpolator->interpolate( currentAltitude );
        currentCoefficients = outputIterator->second.segment( 1, 3 );
        BOOST_CHECK_CLOSE_FRACTION( computedDragCoefficient, currentCoefficients( 0 ),
                                    4.0 * std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( std::fabs( currentCoefficients( 1 ) ), std::numeric_limits< double >::epsilon( ) );
        BOOST_CHECK_SMALL( std::fabs( currentCoefficients( 2 ) ), std::numeric_limits< double >::epsilon( ) );

    }

}

BOOST_AUTO_TEST_SUITE_END( )

}

}


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

#define BOOST_TEST_MODULE shacal2_cipher_test

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <boost/crypto3/block/shacal2.hpp>

#include <boost/crypto3/block/algorithm/encrypt.hpp>
#include <boost/crypto3/block/algorithm/decrypt.hpp>

using namespace boost::crypto3;
using namespace boost::crypto3::block;

BOOST_TEST_DONT_PRINT_LOG_VALUE(block::shacal2<256>::block_type)

BOOST_AUTO_TEST_SUITE(shacal_test_suite)

BOOST_AUTO_TEST_CASE(shacal2_single_block_encrypt1) {
    typedef block::shacal2<256> bct;

    // Test with the equivalent of SHA-256("")
    bct::block_type plaintext = {
        {0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a, 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19}};
    bct::key_type key = {{0x80000000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
    bct cipher(key);
    bct::block_type ciphertext = cipher.encrypt(plaintext);
    bct::block_type expected_ciphertext = {
        {0x79a6dddb, 0xdd946d8f, 0x5e8d0156, 0xf41fc3ea, 0xd69fef65, 0xc9962ac0, 0x8511bf70, 0x1c71eb3c}};

    BOOST_CHECK_EQUAL(ciphertext, expected_ciphertext);

    bct::block_type new_plaintext = cipher.decrypt(ciphertext);

    BOOST_CHECK_EQUAL(plaintext, new_plaintext);
}

BOOST_AUTO_TEST_SUITE_END()


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

    @Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/group.hpp>
#include <boost/simd/function/if_inc.hpp>
#include <boost/simd/function/split.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_zero_else.hpp>
#include <boost/simd/function/is_lez.hpp>
#include <boost/simd/function/is_nez.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/dispatch/meta/upgrade.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( ilogb_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( const A0 & a0)
    {
      using ui16_t =  bd::upgrade_t<A0>;
      const A0 mask =  Constant<A0, 0x7f>();
      A0 n = a0;
      A0 i = One<A0>();
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      n = bitwise_and(mask, shift_right(bitwise_cast<ui16_t>(n), 1));
      i = if_inc(is_nez(n), i);
      return dec(i);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( ilogb_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          )
  {

    BOOST_FORCEINLINE A0 operator() ( const A0 & a0)
    {
      using uA0 = bd::as_integer_t<A0, unsigned>;
      return bitwise_cast<A0>(ilogb(bitwise_cast<uA0>(saturated_(abs)(a0))));
    }
  };

} } }

#endif


#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <map>
#include <stdlib.h>

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

#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>

using namespace std;
using namespace boost;

// data type declaration
typedef boost::geometry::model::d2::point_xy<int> point;
typedef boost::geometry::model::polygon<point> polygon;
typedef boost::geometry::model::box<point> box;

typedef map<string,map<int,vector<polygon*> > > polymap;
typedef map<string,map<int,vector<box*> > > boxmap;
typedef map<string,map<int,vector<string> > > datamap;

// data block
polymap markup;
boxmap outline;
datamap data;
const string tab = "\t";
const string sep =  "\x02";
vector<int> join_idx ;

bool cleanup();
bool readSpatialInput();

bool readSpatialInput() {
    string input_line;
    string record;
    string tile_id;
    string row ;
    vector<string> sp;
    vector<string> fields;

    polygon * shape= NULL;
    box * mbb = NULL;

    int index = 0;
    int pos   = -1;

    while(cin && getline(cin, input_line) && !cin.eof()) {
	//cerr << input_line << endl;
	split( sp,input_line, is_any_of("\t")); // SplitVec == { "hello abc","ABC","aBc goodbye" }

	index = boost::lexical_cast<int>(sp[2]);

	pos = join_idx[index];

	split(fields,sp[1], is_any_of(sep));

	record= boost::algorithm::join(fields, tab);

	shape = new polygon();
	mbb   = new box();
	boost::geometry::read_wkt(fields[pos],*shape);
	boost::geometry::correct(*shape);
	boost::geometry::envelope(*shape, *mbb);
	//assert(boost::geometry::area(*mbb)>=0);
	outline[sp[0]][index].push_back(mbb);
	markup[sp[0]][index].push_back(shape);
	data[sp[0]][index].push_back(record);

	cerr <<"Field Count = " << fields.size() << " , index = " << index << endl;
	fields.clear();
	sp.clear();
    }
    return true;
}
bool cleanup() {
}

int main(int argc, char** argv)
{
    if (argc< 3)
    {
	cerr << "usage: join [shape_idx 1] [shape_idx 2] " <<endl;
	return 0;
    }

    // prepare join parameters
    for (int i =1 ; i <argc ;i++)
	join_idx.push_back(atoi(argv[i]));

    boxmap::iterator iter;
    string key ;
    int size = -1;

    if (readSpatialInput())
    {
	// for each key(tile) in the input stream
	for (iter= outline.begin(); iter != outline.end(); iter++)
	{
	    key  = iter->first;
	    map<int ,vector<box*> > &mbbs = outline[key];
	    map<int ,vector<polygon*> > &shapes = markup[key];

	    size = mbbs.size(); // join cardinality
	    vector<polygon> output;

	    int outerloop_size = (mbbs[0]).size(); //src tile polygon size
	    for (int i =1; i< size ;i++)
	    {
		int innerloop_size = (mbbs[i]).size(); // tar tile polygon size
		for (int j =0; j< outerloop_size; j++)
		{
		    for (int k=0; k< innerloop_size; k++)
		    {
			if (boost::geometry::intersects( *(mbbs[0][j]), *(mbbs[i][k])) &&
				boost::geometry::intersects(*(shapes[0][j]), *(shapes[i][k])) ) // if the mbb && shapes intersects
			{
			    cout << data[key][0][j]<< tab << data[key][i][k] << endl; // cross product
			}
		    }
		}
	    }

	}
	return 0;
    }
    else
	return 1;

}


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Gentry, A., Smyth, D., and Oliver, W. . The Mark IV Supersonic-Hypersonic Arbitrary Body
 *        Program, Volume II - Program Formulation, Douglas Aircraft Aircraft Company, 1973.
 *
 */

#include <string>

#include <boost/bind.hpp>
#include <functional>
#include <boost/lambda/lambda.hpp>
#include <boost/make_shared.hpp>
#include <boost/pointer_cast.hpp>
#include <memory>

#include <Eigen/Geometry>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h"
#include "Tudat/Astrodynamics/Aerodynamics/hypersonicLocalInclinationAnalysis.h"
#include "Tudat/Mathematics/GeometricShapes/compositeSurfaceGeometry.h"
#include "Tudat/Mathematics/GeometricShapes/surfaceGeometry.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{
namespace aerodynamics
{

using Eigen::Vector6d;
using mathematical_constants::PI;

using namespace geometric_shapes;

//! Returns default values of mach number for use in HypersonicLocalInclinationAnalysis.
std::vector< double > getDefaultHypersonicLocalInclinationMachPoints(
        const std::string& machRegime )
{
    std::vector< double > machPoints;

    // Set default points for full hypersonic analysis.
    if ( machRegime == "Full" )
    {
        machPoints.resize( 6 );

        machPoints[ 0 ] = 3.0;
        machPoints[ 1 ] = 4.0;
        machPoints[ 2 ] = 5.0;
        machPoints[ 3 ] = 8.0;
        machPoints[ 4 ] = 10.0;
        machPoints[ 5 ] = 20.0;
    }

    // Set default points for low hypersonic analysis.
    else if ( machRegime == "Low" )
    {
        machPoints.resize( 5 );
        machPoints[ 0 ] = 3.0;
        machPoints[ 1 ] = 4.0;
        machPoints[ 2 ] = 5.0;
        machPoints[ 3 ] = 8.0;
        machPoints[ 4 ] = 10.0;
    }

    // Set default points for high hypersonic analysis.
    else if ( machRegime == "High" )
    {
        machPoints.resize( 4 );
        machPoints[ 0 ] = 5.0;
        machPoints[ 1 ] = 8.0;
        machPoints[ 2 ] = 10.0;
        machPoints[ 3 ] = 20.0;
    }
    return machPoints;
}

//! Returns default values of angle of attack for use in HypersonicLocalInclinationAnalysis.
std::vector< double > getDefaultHypersonicLocalInclinationAngleOfAttackPoints( )
{
    std::vector< double > angleOfAttackPoints;

    // Set number of data points and allocate memory.
    angleOfAttackPoints.resize( 11 );

    // Set default values, 0 to 40 degrees, with steps of 5 degrees.
    for ( int i = 0; i < 11; i++ )
    {
        angleOfAttackPoints[ i ] =
                ( static_cast< double >( i ) * 5.0 * PI / 180.0 );
    }
    return angleOfAttackPoints;
}

//! Returns default values of angle of sideslip for use in HypersonicLocalInclinationAnalysis.
std::vector< double > getDefaultHypersonicLocalInclinationAngleOfSideslipPoints( )
{
    std::vector< double > angleOfSideslipPoints;

    // Set number of data points and allocate memory.
    angleOfSideslipPoints.resize( 2 );

    // Set default values, 0 and 1 degrees.
    angleOfSideslipPoints[ 0 ] = 0.0;
    angleOfSideslipPoints[ 1 ] = 1.0 * PI / 180.0;

    return angleOfSideslipPoints;
}

//! Function that saves the vehicle mesh data used for a HypersonicLocalInclinationAnalysis to a file
void saveVehicleMeshToFile(
        const std::shared_ptr< HypersonicLocalInclinationAnalysis > localInclinationAnalysis,
        const std::string directory,
        const std::string filePrefix )
{
    std::vector< boost::multi_array< Eigen::Vector3d, 2 > > meshPoints =
            localInclinationAnalysis->getMeshPoints( );
    std::vector< boost::multi_array< Eigen::Vector3d, 2 > > meshSurfaceNormals =
            localInclinationAnalysis->getPanelSurfaceNormals( );

//    boost::array< int, 3 > independentVariables;
//    independentVariables[ 0 ] = 0;
//    independentVariables[ 1 ] = 6;
//    independentVariables[ 2 ] = 0;

//    std::vector< std::vector< std::vector< double > > > pressureCoefficients =
//            localInclinationAnalysis->getPressureCoefficientList( independentVariables );

    int counter = 0;
    std::map< int, Eigen::Vector3d > meshPointsList;
    std::map< int, Eigen::Vector3d > surfaceNormalsList;
//    std::map< int, Eigen::Vector1d > pressureCoefficientsList;

    for( unsigned int i = 0; i < meshPoints.size( ); i++ )
    {
        for( unsigned int j = 0; j < meshPoints.at( i ).shape( )[ 0 ] - 1; j++ )
        {
            for( unsigned int k = 0; k < meshPoints.at( i ).shape( )[ 1 ] - 1; k++ )
            {
                meshPointsList[ counter ] = meshPoints[ i ][ j ][ k ];
                surfaceNormalsList[ counter ] = meshSurfaceNormals[ i ][ j ][ k ];
//                pressureCoefficientsList[ counter ] = ( Eigen::Vector1d( ) << pressureCoefficients[ i ][ j ][ k ] ).finished( );
                counter++;
            }
        }
    }

    input_output::writeDataMapToTextFile(
                meshPointsList, filePrefix + "ShapeFile.dat", directory );
    input_output::writeDataMapToTextFile(
                surfaceNormalsList, filePrefix + "SurfaceNormalFile.dat", directory );

//    input_output::writeDataMapToTextFile(
//                pressureCoefficientsList, filePrefix + "pressureCoefficientFile.dat", directory );
}

//! Default constructor.
HypersonicLocalInclinationAnalysis::HypersonicLocalInclinationAnalysis(
        const std::vector< std::vector< double > >& dataPointsOfIndependentVariables,
        const std::shared_ptr< SurfaceGeometry > inputVehicleSurface,
        const std::vector< int >& numberOfLines,
        const std::vector< int >& numberOfPoints,
        const std::vector< bool >& invertOrders,
        const std::vector< std::vector< int > >& selectedMethods,
        const double referenceArea,
        const double referenceLength,
        const Eigen::Vector3d& momentReferencePoint,
        const bool savePressureCoefficients )
    : AerodynamicCoefficientGenerator< 3, 6 >(
          dataPointsOfIndependentVariables, referenceLength, referenceArea, referenceLength,
          momentReferencePoint, { mach_number_dependent, angle_of_attack_dependent, angle_of_sideslip_dependent },true, false ),
      stagnationPressureCoefficient( 2.0 ),
      ratioOfSpecificHeats( 1.4 ),
      selectedMethods_( selectedMethods ),
      savePressureCoefficients_( savePressureCoefficients )
{
    // Set geometry if it is a single surface.
    if ( std::dynamic_pointer_cast< SingleSurfaceGeometry > ( inputVehicleSurface ) !=
         std::shared_ptr< SingleSurfaceGeometry >( ) )
    {
        // Set number of geometries and allocate memory.
        vehicleParts_.resize( 1 );

        vehicleParts_[ 0 ] = std::make_shared< LawgsPartGeometry >( );
        vehicleParts_[ 0 ]->setReversalOperator( invertOrders[ 0 ] );

        // If part is not already a LaWGS part, convert it.
        if ( std::dynamic_pointer_cast< LawgsPartGeometry >
             ( inputVehicleSurface ) ==
             std::shared_ptr< LawgsPartGeometry >( ) )
        {
            // Convert geometry to LaWGS surface mesh and set in vehicleParts_ list.
            vehicleParts_[ 0 ]->setMesh(
                        std::dynamic_pointer_cast< SingleSurfaceGeometry > ( inputVehicleSurface ),
                        numberOfLines[ 0 ], numberOfPoints[ 0 ] );
        }

        // Else, set geometry directly.
        else
        {
            vehicleParts_[ 0 ] = std::dynamic_pointer_cast< LawgsPartGeometry >
                    ( inputVehicleSurface );
        }
    }

    // Set geometry if it is a composite surface.
    else if ( std::dynamic_pointer_cast< CompositeSurfaceGeometry >( inputVehicleSurface ) !=
              std::shared_ptr< CompositeSurfaceGeometry >( ) )
    {
        // Dynamic cast to composite surface geometry for further processing.
        std::shared_ptr< CompositeSurfaceGeometry > compositeSurfaceGeometry_ =
                std::dynamic_pointer_cast< CompositeSurfaceGeometry >( inputVehicleSurface );

        // Set number of geometries and allocate memory.
        int numberOfVehicleParts =
                compositeSurfaceGeometry_->getNumberOfSingleSurfaceGeometries( );
        vehicleParts_.resize( numberOfVehicleParts );

        // Iterate through all parts and set them in vehicleParts_ list.
        for ( int i = 0; i < numberOfVehicleParts; i++ )
        {
            // If part is not already a LaWGS part, convert it.
            if ( std::dynamic_pointer_cast< LawgsPartGeometry >
                 ( compositeSurfaceGeometry_->getSingleSurfaceGeometry( i ) ) ==
                 std::shared_ptr< LawgsPartGeometry >( ) )
            {
                vehicleParts_[ i ] = std::make_shared< LawgsPartGeometry >( );
                vehicleParts_[ i ]->setReversalOperator( invertOrders[ i ] );

                // Convert geometry to LaWGS and set in list.
                vehicleParts_[ i ]->setMesh(
                            compositeSurfaceGeometry_->getSingleSurfaceGeometry( i ),
                            numberOfLines[ i ], numberOfPoints[ i ] );
            }

            // Else, set geometry directly.
            else
            {
                vehicleParts_[ i ] = std::dynamic_pointer_cast< LawgsPartGeometry >(
                            compositeSurfaceGeometry_->getSingleSurfaceGeometry( i ) );
            }
        }
    }

    // Allocate memory for panel inclinations and pressureCoefficient_.
    inclination_.resize( vehicleParts_.size( ) );
    pressureCoefficient_.resize( vehicleParts_.size( ) );
    for ( unsigned int i = 0 ; i < vehicleParts_.size( ); i++ )
    {
        inclination_[ i ].resize( vehicleParts_[ i ]->getNumberOfLines( ) );
        pressureCoefficient_[ i ].resize( vehicleParts_[ i ]->getNumberOfLines( ) );
        for ( int j = 0 ; j < vehicleParts_[ i ]->getNumberOfLines( ) ; j++ )
        {
            inclination_[ i ][ j ].resize( vehicleParts_[ i ]->getNumberOfPoints( ) );
            pressureCoefficient_[ i ][ j ].resize( vehicleParts_[ i ]->getNumberOfPoints( ) );
        }
    }

    boost::array< int, 3 > numberOfPointsPerIndependentVariables;
    for( int i = 0; i < 3; i++ )
    {
        numberOfPointsPerIndependentVariables[ i ] =
                dataPointsOfIndependentVariables_[ i ].size( );
    }

    isCoefficientGenerated_.resize( numberOfPointsPerIndependentVariables );

    std::fill( isCoefficientGenerated_.origin( ),
               isCoefficientGenerated_.origin( ) + isCoefficientGenerated_.num_elements( ), 0 );

    generateCoefficients( );
    createInterpolator( );
}

//! Get aerodynamic coefficients.
Vector6d HypersonicLocalInclinationAnalysis::getAerodynamicCoefficientsDataPoint(
        const boost::array< int, 3 > independentVariables )
{
    if( isCoefficientGenerated_( independentVariables ) == 0 )
    {
        determineVehicleCoefficients( independentVariables );
    }

    // Return requested coefficients.
    return aerodynamicCoefficients_( independentVariables );
}

//! Generate aerodynamic database.
void HypersonicLocalInclinationAnalysis::generateCoefficients( )
{
    // Allocate variable to pass to coefficient determination for independent
    // variable indices.
    boost::array< int, 3 > independentVariableIndices;

    // Iterate over all combinations of independent variables.
    for ( unsigned int i = 0 ; i < dataPointsOfIndependentVariables_[ 0 ].size( ) ; i++ )
    {
        independentVariableIndices[ 0 ] = i;
        for ( unsigned  int j = 0 ; j < dataPointsOfIndependentVariables_[
              1 ].size( ) ; j++ )
        {
            independentVariableIndices[ 1 ] = j;
            for ( unsigned  int k = 0 ; k < dataPointsOfIndependentVariables_[
                  2 ].size( ) ; k++ )
            {
                independentVariableIndices[ 2 ] = k;

                determineVehicleCoefficients( independentVariableIndices );
            }
        }
    }
}

//! Generate aerodynamic coefficients at a single set of independent variables.
void HypersonicLocalInclinationAnalysis::determineVehicleCoefficients(
        const boost::array< int, 3 > independentVariableIndices )
{
    // Declare coefficients vector and initialize to zeros.
    Vector6d coefficients = Vector6d::Zero( );

    // Loop over all vehicle parts, calculate aerodynamic coefficients and add
    // to aerodynamicCoefficients_.
    for ( unsigned int i = 0 ; i < vehicleParts_.size( ) ; i++ )
    {
        coefficients += determinePartCoefficients( i, independentVariableIndices );
    }

    if( savePressureCoefficients_ )
    {
        pressureCoefficientList_[ independentVariableIndices ] = pressureCoefficient_;
    }

    aerodynamicCoefficients_( independentVariableIndices ) = coefficients;
    isCoefficientGenerated_( independentVariableIndices ) = 1;
}

//! Determine aerodynamic coefficients of a single vehicle part.
Vector6d HypersonicLocalInclinationAnalysis::determinePartCoefficients(
        const int partNumber, const boost::array< int, 3 > independentVariableIndices )
{
    // Declare and determine angles of attack and sideslip for analysis.
    double angleOfAttack =  dataPointsOfIndependentVariables_[ 1 ]
            [ independentVariableIndices[ 1 ] ];

    double angleOfSideslip =  dataPointsOfIndependentVariables_[ 2 ]
            [ independentVariableIndices[ 2 ] ];

    // Declare partCoefficient vector.
    Vector6d partCoefficients = Vector6d::Zero( );

    // Check whether the inclinations of the vehicle part have already been computed.
    if ( previouslyComputedInclinations_.count( std::pair< double, double >(
                                                    angleOfAttack, angleOfSideslip ) ) == 0 )
    {
        // Determine panel inclinations for part.
        determineInclinations( angleOfAttack, angleOfSideslip );

        // Add panel inclinations to container
        previouslyComputedInclinations_[ std::pair< double, double >(
                    angleOfAttack, angleOfSideslip ) ] = inclination_;
    }

    else
    {
        // Fetch inclinations from container
        inclination_ = previouslyComputedInclinations_[ std::pair< double, double >(
                    angleOfAttack, angleOfSideslip ) ];
    }

    // Set pressureCoefficient_ array for given independent variables.
    determinePressureCoefficients( partNumber, independentVariableIndices );

    // Calculate force coefficients from pressure coefficients.
    partCoefficients.segment( 0, 3 ) = calculateForceCoefficients( partNumber );

    // Calculate moment coefficients from pressure coefficients.
    partCoefficients.segment( 3, 3 ) = calculateMomentCoefficients( partNumber );

    return partCoefficients;
}

//! Determine the pressure coefficients on a single vehicle part.
void HypersonicLocalInclinationAnalysis::determinePressureCoefficients(
        const int partNumber, const boost::array< int, 3 > independentVariableIndices )
{
    // Retrieve Mach number.
    double machNumber = dataPointsOfIndependentVariables_[ 0 ]
            [ independentVariableIndices[ 0 ] ];

    // Determine stagnation point pressure coefficients. Value is computed once
    // here to prevent its calculation in inner loop.
    stagnationPressureCoefficient = computeStagnationPressure(
                machNumber, ratioOfSpecificHeats );

    updateCompressionPressures( machNumber, partNumber );
    updateExpansionPressures( machNumber, partNumber );
}

//! Determine force coefficients from pressure coefficients.
Eigen::Vector3d HypersonicLocalInclinationAnalysis::calculateForceCoefficients(
        const int partNumber )
{
    // Declare force coefficient vector and intialize to zeros.
    Eigen::Vector3d forceCoefficients = Eigen::Vector3d::Zero( );

    // Loop over all panels and add pressures, scaled by panel area, to force
    // coefficients.
    for ( int i = 0 ; i < vehicleParts_[ partNumber ]->getNumberOfLines( ) - 1 ; i++ )
    {
        for ( int j = 0 ; j < vehicleParts_[ partNumber ]->getNumberOfPoints( ) - 1 ; j++)
        {
            forceCoefficients -=
                    pressureCoefficient_[ partNumber ][ i ][ j ] *
                    vehicleParts_[ partNumber ]->getPanelArea( i, j ) *
                    vehicleParts_[ partNumber ]->getPanelSurfaceNormal( i, j );
        }
    }

    // Normalize result by reference area.
    forceCoefficients /= referenceArea_;

    return forceCoefficients;
}

//! Determine moment coefficients from pressure coefficients.
Eigen::Vector3d HypersonicLocalInclinationAnalysis::calculateMomentCoefficients(
        const int partNumber )
{
    // Declare moment coefficient vector and intialize to zeros.
    Eigen::Vector3d momentCoefficients = Eigen::Vector3d::Zero( );

    // Declare moment arm for panel moment determination.
    Eigen::Vector3d referenceDistance;

    // Loop over all panels and add moments due pressures.
    for ( int i = 0 ; i < vehicleParts_[ partNumber ]->getNumberOfLines( ) - 1 ; i++ )
    {
        for ( int j = 0 ; j < vehicleParts_[ partNumber ]->getNumberOfPoints( ) - 1 ; j++ )
        {
            // Determine moment arm for given panel centroid.
            referenceDistance = ( vehicleParts_[ partNumber ]->getPanelCentroid( i, j ) -
                                  momentReferencePoint_ );

            momentCoefficients -=
                    pressureCoefficient_[ partNumber ][ i ][ j ] *
                    vehicleParts_[ partNumber ]->getPanelArea( i, j ) *
                    ( referenceDistance.cross( vehicleParts_[ partNumber ]->
                                               getPanelSurfaceNormal( i, j ) ) );
        }
    }

    // Scale result by reference length and area.
    momentCoefficients /= ( referenceLength_ * referenceArea_ );

    return momentCoefficients;
}

//! Determines the inclination angle of panels on a single part.
void HypersonicLocalInclinationAnalysis::determineInclinations( const double angleOfAttack,
                                                                const double angleOfSideslip )
{
    // Declare free-stream velocity vector.
    Eigen::Vector3d freestreamVelocityDirection;

    // Set freestream velocity vector in body frame.
    double freestreamVelocityDirectionX = cos( angleOfAttack )* cos( angleOfSideslip );
    double freestreamVelocityDirectionY = sin( angleOfSideslip );
    double freestreamVelocityDirectionZ = sin( angleOfAttack ) * cos( angleOfSideslip );
    freestreamVelocityDirection( 0 ) = freestreamVelocityDirectionX;
    freestreamVelocityDirection( 1 ) = freestreamVelocityDirectionY;
    freestreamVelocityDirection( 2 ) = freestreamVelocityDirectionZ;

    // Declare cosine of inclination angle.
    double cosineOfInclination;

    // Loop over all panels of given vehicle part and set inclination angles.
    for( unsigned int k = 0; k < vehicleParts_.size( ); k++ )
    {
        for ( int i = 0 ; i < vehicleParts_[ k ]->getNumberOfLines( ) - 1 ; i++ )
        {
            for ( int j = 0 ; j < vehicleParts_[ k ]->getNumberOfPoints( ) - 1 ; j++ )
            {

                // Determine cosine of inclination angle from inner product between
                // surface normal and free-stream direction.
                cosineOfInclination = vehicleParts_[ k ]->
                        getPanelSurfaceNormal( i, j ).
                        dot( freestreamVelocityDirection );

                // Set inclination angle.
                inclination_[ k ][ i ][ j ] = PI / 2.0 - acos( cosineOfInclination );
            }
        }
    }
}

//! Determine compression pressure coefficients on all parts.
void HypersonicLocalInclinationAnalysis::updateCompressionPressures( const double machNumber,
                                                                     const int partNumber )
{
    int method = selectedMethods_[ 0 ][ partNumber ];

    std::function< double( double ) > pressureFunction;

    // Switch to analyze part using correct method.
    switch( method )
    {
    case 0:
        pressureFunction =
                std::bind( aerodynamics::computeNewtonianPressureCoefficient, std::placeholders::_1 );
        break;

    case 1:
        pressureFunction =
                std::bind( aerodynamics::computeModifiedNewtonianPressureCoefficient, std::placeholders::_1,
                           stagnationPressureCoefficient );
        break;

    case 2:
        // Method currently disabled.
        break;

    case 3:
        // Method currently disabled.
        break;

    case 4:
        pressureFunction =
                std::bind( aerodynamics::computeEmpiricalTangentWedgePressureCoefficient, std::placeholders::_1,
                           machNumber );
        break;

    case 5:
        pressureFunction =
                std::bind( aerodynamics::computeEmpiricalTangentConePressureCoefficient, std::placeholders::_1,
                           machNumber );
        break;

    case 6:
        pressureFunction =
                std::bind( aerodynamics::computeModifiedDahlemBuckPressureCoefficient, std::placeholders::_1,
                           machNumber );
        break;

    case 7:
        pressureFunction =
                std::bind( aerodynamics::computeVanDykeUnifiedPressureCoefficient, std::placeholders::_1,
                           machNumber, ratioOfSpecificHeats, 1 );
        break;

    case 8:
        pressureFunction =
                std::bind( aerodynamics::computeSmythDeltaWingPressureCoefficient, std::placeholders::_1,
                           machNumber );
        break;

    case 9:
        pressureFunction =
                std::bind( aerodynamics::computeHankeyFlatSurfacePressureCoefficient, std::placeholders::_1,
                           machNumber );
        break;

    default:
        break;
    }

    for ( int i = 0 ; i < vehicleParts_[ partNumber ]->getNumberOfLines( ) - 1; i++ )
    {
        for ( int j = 0 ; j < vehicleParts_[ partNumber ]->getNumberOfPoints( ) - 1 ; j++ )
        {
            if ( inclination_[ partNumber ][ i ][ j ] > 0 )
            {
                // If panel inclination is positive, calculate pressure coefficient.
                pressureCoefficient_[ partNumber ][ i ][ j ] =
                        pressureFunction( inclination_[ partNumber ][ i ][ j ] );
            }
        }
    }
}

//! Determines expansion pressure coefficients on all parts.
void HypersonicLocalInclinationAnalysis::updateExpansionPressures( const double machNumber,
                                                                   const int partNumber )
{
    // Get analysis method of part to analyze.
    int method = selectedMethods_[ 1 ][ partNumber ];

    if ( method == 0 || method == 1 || method == 4 )
    {
        std::function< double( ) > pressureFunction;
        switch( method )
        {
        case 0:
            pressureFunction = std::bind( &aerodynamics::computeVacuumPressureCoefficient,
                                          machNumber, ratioOfSpecificHeats );
            break;

        case 1:
            pressureFunction = [ ]( ){ return 0.0; };
            break;

        case 4:
            pressureFunction = std::bind( &aerodynamics::computeHighMachBasePressure,
                                          machNumber );
            break;

        }

        // Iterate over all panels on part.
        for ( int i = 0 ; i < vehicleParts_[ partNumber ]->getNumberOfLines( ) - 1 ; i++ )
        {
            for ( int j = 0 ; j < vehicleParts_[ partNumber ]->getNumberOfPoints( ) - 1 ; j++ )
            {
                if ( inclination_[ partNumber ][ i ][ j ] <= 0 )
                {
                    // If panel inclination is negative, calculate pressure using
                    // Van Dyke unified method.
                    pressureCoefficient_[ partNumber ][ i ][ j ] =
                            pressureFunction( );
                }
            }
        }
    }

    else if( method == 3 || method == 5 || method == 6 )
    {

        std::function< double( double ) > pressureFunction;

        // Declare local variable.
        double freestreamPrandtlMeyerFunction;

        // Switch to analyze part using correct method.
        switch( method )
        {
        case 3:
            // Calculate freestream Prandtl-Meyer function.
            freestreamPrandtlMeyerFunction = aerodynamics::computePrandtlMeyerFunction(
                        machNumber, ratioOfSpecificHeats );
            pressureFunction =
                    std::bind( &aerodynamics::computePrandtlMeyerFreestreamPressureCoefficient,
                               std::placeholders::_1, machNumber, ratioOfSpecificHeats,
                               freestreamPrandtlMeyerFunction );
            break;

        case 5:
            pressureFunction =
                    std::bind( &aerodynamics::computePrandtlMeyerFreestreamPressureCoefficient,
                               std::placeholders::_1, machNumber, ratioOfSpecificHeats, -1 );
            break;

        case 6:
            pressureFunction = std::bind( &aerodynamics::computeAcmEmpiricalPressureCoefficient,
                                          std::placeholders::_1, machNumber );
            break;
        }

        // Iterate over all panels on part.
        for ( int i = 0 ; i < vehicleParts_[ partNumber ]->getNumberOfLines( ) - 1 ; i++ )
        {
            for ( int j = 0 ; j < vehicleParts_[ partNumber ]->getNumberOfPoints( ) - 1 ; j++ )
            {
                if ( inclination_[ partNumber ][ i ][ j ] <= 0 )
                {
                    // If panel inclination is negative, calculate pressure using
                    // Van Dyke unified method.
                    pressureCoefficient_[ partNumber ][ i ][ j ] =
                            pressureFunction( inclination_[ partNumber ][ i ][ j ] );
                }
            }
        }
    }

    else
    {
        std::string errorMessage = "Error, expansion local inclination method number "
                + std::to_string( method ) + " not recognized";
        throw std::runtime_error( errorMessage );
    }
}

} // namespace aerodynamics
} // namespace tudat


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef FPUTILS_LOG_SPACE_MATRIX_MULTIPLY_HPP
#define FPUTILS_LOG_SPACE_MATRIX_MULTIPLY_HPP

#include <boost/numeric/ublas/matrix.hpp>

namespace fputils {
	typedef boost::numeric::ublas::matrix< double > Matrix ;
	typedef boost::numeric::ublas::scalar_matrix<double> ConstantMatrix ;

	// function: log_multiply_exp
	// Work out the product of two matrices, but interpret all quantities as being in natural log space.
	// (Thus the result at elt. i,j is the log of the i,j th entry of the product of the matrices
	// obtained by exponentiating each elt of the left and right matrices).
	Matrix log_space_matrix_multiply( Matrix const& left, Matrix const& right ) ;
}

#endif


#define BOOST_TEST_MODULE "test_omp_global_pwmcos_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/math/math.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/omp/System.hpp>
#include <mjolnir/omp/RandomNumberGenerator.hpp>
#include <mjolnir/omp/UnlimitedGridCellList.hpp>
#include <mjolnir/omp/PWMcosInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>

BOOST_AUTO_TEST_CASE(omp_PWMcos_calc_force)
{
    namespace test = mjolnir::test;
    constexpr double tol = 1e-8;
    mjolnir::LoggerManager::set_default_logger("test_omp_pwmcos_interaction.log");

    using omp_traits_type = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using seq_traits_type = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = double;
    using coordinate_type  = typename omp_traits_type::coordinate_type;
    using boundary_type    = typename omp_traits_type::boundary_type;
    using topology_type    = mjolnir::Topology;

    using potential_type   = mjolnir::PWMcosPotential<real_type>;

    using omp_system_type         = mjolnir::System<omp_traits_type>;
    using omp_parameter_list_type = mjolnir::PWMcosParameterList<omp_traits_type>;
    using omp_interaction_type    = mjolnir::PWMcosInteraction<omp_traits_type>;
    using omp_partition_type      = mjolnir::UnlimitedGridCellList<omp_traits_type, potential_type>;

    using seq_system_type         = mjolnir::System<seq_traits_type>;
    using seq_parameter_list_type = mjolnir::PWMcosParameterList<seq_traits_type>;
    using seq_partition_type      = mjolnir::UnlimitedGridCellList<seq_traits_type, potential_type>;
    using seq_interaction_type    = mjolnir::PWMcosInteraction<seq_traits_type>;

    const int max_number_of_threads = omp_get_max_threads();
    BOOST_TEST_WARN(max_number_of_threads > 2);
    BOOST_TEST_MESSAGE("maximum number of threads = " << max_number_of_threads);

    //         5  6
    //         o--o   |
    //             \ 7|
    // o 0          o |
    //  \      8  9/  |
    //   o 1   o--o   |
    //  /          \10|
    // o 2          o |
    //  \     11 12/  |
    //   o 3   o--o   |
    //  /          \13|
    // o 4          o |
    //        14 15/  |
    //         o--o   |

    std::mt19937 rng(123456789);

    for(int num_thread=1; num_thread<=max_number_of_threads; ++num_thread)
    {
        omp_set_num_threads(num_thread);
        BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

        omp_parameter_list_type omp_parameter_list(1.0, 0.1, 1.0, 0.0, 5.0,
            std::vector<typename omp_parameter_list_type::contact_parameter_type>{
                typename omp_parameter_list_type::contact_parameter_type{1, 0, 2, 6.0, 0.0, 5.0, 3.14 * 0.5, 3.14 * 0.667, 3.14 * 0.667, 0.0, {{1.0, 1.0, 1.0, 1.0}} },
                typename omp_parameter_list_type::contact_parameter_type{2, 1, 3, 6.0, 0.0, 5.0, 3.14 * 0.5, 3.14 * 0.667, 3.14 * 0.667, 0.0, {{1.0, 1.0, 1.0, 1.0}} },
                typename omp_parameter_list_type::contact_parameter_type{3, 2, 4, 6.0, 0.0, 5.0, 3.14 * 0.5, 3.14 * 0.667, 3.14 * 0.667, 0.0, {{1.0, 1.0, 1.0, 1.0}} }
            },
            std::vector<typename omp_parameter_list_type::dna_parameter_type>{
                typename omp_parameter_list_type::dna_parameter_type{omp_parameter_list_type::base_kind::A,  8,  9,  5, 11},
                typename omp_parameter_list_type::dna_parameter_type{omp_parameter_list_type::base_kind::A, 11, 15,  8, 14},
            },
            {},
            typename omp_parameter_list_type::ignore_molecule_type("Nothing"),
            typename omp_parameter_list_type::ignore_group_type({}));

        seq_parameter_list_type seq_parameter_list(1.0, 0.1, 1.0, 0.0, 5.0,
            std::vector<typename seq_parameter_list_type::contact_parameter_type>{
                typename seq_parameter_list_type::contact_parameter_type{1, 0, 2, 6.0, 0.0, 5.0, 3.14 * 0.5, 3.14 * 0.667, 3.14 * 0.667, 0.0, {{1.0, 1.0, 1.0, 1.0}} },
                typename seq_parameter_list_type::contact_parameter_type{2, 1, 3, 6.0, 0.0, 5.0, 3.14 * 0.5, 3.14 * 0.667, 3.14 * 0.667, 0.0, {{1.0, 1.0, 1.0, 1.0}} },
                typename seq_parameter_list_type::contact_parameter_type{3, 2, 4, 6.0, 0.0, 5.0, 3.14 * 0.5, 3.14 * 0.667, 3.14 * 0.667, 0.0, {{1.0, 1.0, 1.0, 1.0}} }
            },
            std::vector<typename seq_parameter_list_type::dna_parameter_type>{
                typename seq_parameter_list_type::dna_parameter_type{seq_parameter_list_type::base_kind::A,  8,  9,  5, 11},
                typename seq_parameter_list_type::dna_parameter_type{seq_parameter_list_type::base_kind::A, 11, 15,  8, 14},
            },
            {},
            typename seq_parameter_list_type::ignore_molecule_type("Nothing"),
            typename seq_parameter_list_type::ignore_group_type({}));

        omp_system_type omp_sys(16, boundary_type{});
        test::clear_everything(omp_sys);

        omp_sys.position( 0) = mjolnir::math::make_coordinate<coordinate_type>( 0.0,  3.3, 0.0);
        omp_sys.position( 1) = mjolnir::math::make_coordinate<coordinate_type>( 1.9,  1.6, 0.0);
        omp_sys.position( 2) = mjolnir::math::make_coordinate<coordinate_type>( 0.0,  0.0, 0.0);
        omp_sys.position( 3) = mjolnir::math::make_coordinate<coordinate_type>( 1.9, -1.6, 0.0);
        omp_sys.position( 4) = mjolnir::math::make_coordinate<coordinate_type>( 0.0, -3.3, 0.0);
        omp_sys.position( 5) = mjolnir::math::make_coordinate<coordinate_type>( 6.9,  5.0, 0.0);
        omp_sys.position( 6) = mjolnir::math::make_coordinate<coordinate_type>( 8.8,  5.0, 0.0);
        omp_sys.position( 7) = mjolnir::math::make_coordinate<coordinate_type>(12.0,  3.3, 0.0);
        omp_sys.position( 8) = mjolnir::math::make_coordinate<coordinate_type>( 6.9,  1.6, 0.0);
        omp_sys.position( 9) = mjolnir::math::make_coordinate<coordinate_type>( 8.8,  1.6, 0.0);
        omp_sys.position(10) = mjolnir::math::make_coordinate<coordinate_type>(12.0,  0.0, 0.0);
        omp_sys.position(11) = mjolnir::math::make_coordinate<coordinate_type>( 6.9, -1.6, 0.0);
        omp_sys.position(12) = mjolnir::math::make_coordinate<coordinate_type>( 8.8, -1.6, 0.0);
        omp_sys.position(13) = mjolnir::math::make_coordinate<coordinate_type>(12.0, -3.3, 0.0);
        omp_sys.position(14) = mjolnir::math::make_coordinate<coordinate_type>( 6.9, -5.0, 0.0);
        omp_sys.position(15) = mjolnir::math::make_coordinate<coordinate_type>( 8.8, -5.0, 0.0);

        test::apply_random_perturbation(omp_sys, rng, 0.1);

        topology_type topol(16);
        topol.construct_molecules();

        // init seq one with the same coordinates
        seq_system_type seq_sys(16, boundary_type{});
        test::clear_everything(seq_sys);
        for(std::size_t i=0; i<seq_sys.size(); ++i)
        {
            seq_sys.mass(i)     = omp_sys.mass(i);
            seq_sys.position(i) = omp_sys.position(i);
            seq_sys.velocity(i) = omp_sys.velocity(i);
            seq_sys.force(i)    = omp_sys.force(i);
            seq_sys.name(i)     = omp_sys.name(i);
            seq_sys.group(i)    = omp_sys.group(i);
        }

        omp_interaction_type omp_interaction(potential_type{}, std::move(omp_parameter_list),
            mjolnir::SpatialPartition<omp_traits_type, potential_type>(
                mjolnir::make_unique<omp_partition_type>()));

        seq_interaction_type seq_interaction(potential_type{}, std::move(seq_parameter_list),
            mjolnir::SpatialPartition<seq_traits_type, potential_type>(
                mjolnir::make_unique<seq_partition_type>()));

        omp_interaction.initialize(omp_sys, topol);
        seq_interaction.initialize(seq_sys, topol);

        test::check_force_consistency              (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_energy_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_virial_consistency   (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_energy_virial_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_energy_consistency             (omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
    }
}


#include <mex.h>
#include <iostream>
#include <Eigen/Dense>
#include "drakeUtil.h"
#include "RigidBodyManipulator.h"

using namespace Eigen;
using namespace std;

/*
 * A C version of the getCMM function
 */

void mexFunction( int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[] ) {

  if (nrhs < 2) {
    mexErrMsgIdAndTxt("Drake:getCMMmex:NotEnoughInputs","Usage getCMMmex(model_ptr,q_cache) or getCMMmex(model_ptr,q_cache,qd)");
  }

  // first get the model_ptr back from matlab
  RigidBodyManipulator *model= (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);

  Map<VectorXd> q(mxGetPrSafe(prhs[1]), model->num_positions);
  for (int i = 0; i < model->num_positions; i++) {
    if (q[i] - model->cached_q[i] > 1e-8 || q[i] - model->cached_q[i] < -1e-8) {
      mexErrMsgIdAndTxt("Drake:getCMMmex:InvalidKinematics","This kinsol is no longer valid.  Somebody has called doKinematics with a different q since the solution was computed.");
    }
  }

  plhs[0] = mxCreateDoubleMatrix(6,model->num_positions,mxREAL);
  Map<MatrixXd> A(mxGetPrSafe(plhs[0]),6,model->num_positions);
  plhs[1] = mxCreateDoubleMatrix(6,model->num_positions,mxREAL);
  Map<MatrixXd> Adot(mxGetPrSafe(plhs[1]),6,model->num_positions);

  if (nrhs > 2) {
    Map<VectorXd> qd(mxGetPrSafe(prhs[2]), model->num_velocities);
    model->getCMM(q,qd,A,Adot);
  }
  else {
    VectorXd zeros = VectorXd::Zero(model->num_positions);
    Map<VectorXd> qd(zeros.data(), model->num_velocities);
    model->getCMM(q,qd,A,Adot);
  }
}


#include "../../Headers/stdafx.h"
#include "JvCryption.h"

// Cryption
/*#if __VERSION >= 1700
#define g_private_key 0x1207500120128966
#elif __VERSION == 1298
#define g_private_key 0x1234567890123456
#else
#define g_private_key 0x1257091582190465
#endif*/

#define g_private_key 0x7467148522014785
#include <boost/random.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
void CJvCryption::Init() { m_tkey = m_public_key ^ g_private_key; }

boost::mt19937 gen;

unsigned long long CJvCryption::GenerateKey()
{
#ifdef USE_CRYPTION
	// because of their sucky encryption method, 0 means it effectively won't be encrypted.
	// We don't want that happening...
	do
	{
		boost::uniform_int<unsigned long long> uInt64Dist(0, std::numeric_limits<uint64_t>::max());
		boost::variate_generator<boost::mt19937&, boost::uniform_int<unsigned long long> > getRand(gen, uInt64Dist);
		m_public_key = getRand();
	} while (!m_public_key);
#endif

	//TRACE("Generated public key for session : (%X)", &m_public_key);
	return m_public_key;
}

void CJvCryption::JvEncryptionFast(int len, unsigned char *datain, unsigned char *dataout)
{
	unsigned char *pkey, lkey, rsk;
	int rkey = 2157;

	pkey = (unsigned char *)&m_tkey;
	lkey = (len * 157) & 0xff;

	for (int i = 0; i < len; i++)
	{
		rsk = (rkey >> 8) & 0xff;
		dataout[i] = ((datain[i] ^ rsk) ^ pkey[(i % 8)]) ^ lkey;
		rkey *= 2171;
	}
}

void CJvCryption::JvEncryptionFastBetweenIndex(int len, int start, int end, unsigned char *datain, unsigned char *dataout)
{
	unsigned char *pkey, lkey, rsk;
	int rkey = 2157;

	pkey = (unsigned char *)&m_tkey;
	lkey = (len * 157) & 0xff;

	for (int i = start; i < start + len; i++)
	{
		rsk = (rkey >> 8) & 0xff;
		dataout[i] = ((datain[i] ^ rsk) ^ pkey[((i - start) % 8)]) ^ lkey;
		rkey *= 2171;
	}
}

int CJvCryption::JvDecryptionWithCRC32(int len, unsigned char *datain, unsigned char *dataout)
{
	int result;
	JvDecryptionFast(len, datain, dataout);

	if (crc32(dataout, len - 4, -1) == *(unsigned __int32 *)(len - 4 + dataout))
		result = len - 4;
	else
		result = -1;

	return result;
}


#ifndef BODY_POSE_GENERATOR_H
#define BODY_POSE_GENERATOR_H

#include <random>

#include <Eigen/Geometry>

struct pose_generator_config
{
  double stand_probability;
  double both_leg_average;
  double both_leg_dispersion;
  double body_average;
  double body_dispersion;
  double one_side_average;
  double one_side_dispersion;
};

class pose_generator
{
  std::bernoulli_distribution stand_dist_;
  std::normal_distribution<> both_leg_dist_;
  std::normal_distribution<> body_dist_;
  std::normal_distribution<> one_side_dist_;
  std::default_random_engine engine_ {std::random_device{}()};
  Eigen::Vector3d head_ {};
  Eigen::Vector3d right_hand_ {};
  Eigen::Vector3d left_hand_ {};
  Eigen::Vector3d right_knee_ {};
  Eigen::Vector3d left_knee_ {};

public:
  pose_generator(const pose_generator_config& conf);

  void update();

  Eigen::Vector3d get_head() const noexcept
  {
    return head_;
  }

  Eigen::Vector3d get_right_hand() const noexcept
  {
    return right_hand_;
  }

  Eigen::Vector3d get_left_hand() const noexcept
  {
    return left_hand_;
  }

  Eigen::Vector3d get_right_knee() const noexcept
  {
    return right_knee_;
  }

  Eigen::Vector3d get_left_knee() const noexcept
  {
    return left_knee_;
  }
};

#endif