#ifndef AUTOENCODER_H
#define AUTOENCODER_H

#include "../../core/utility.hpp"
#include "../../eigen/eigen.hpp"

#ifdef OCV_TEST_AUTOENCODER
#include "../utility/gradient_checking.hpp"
#endif

#include "../../profile/measure.hpp"
#include "propagation.hpp"

#include <Eigen/Dense>

#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Dense>

#include <fstream>
#include <random>

/*! \file autoencoder.hpp
    \brief implement the algorithm--autoencoder based on\n
    the description of UFLDL, these codes are develop based\n
    on the example on the website(http://eric-yuan.me/simple-deep-network/)
*/

/*!
 *  \addtogroup ocv
 *  @{
 */
namespace ocv{

/*!
 *  \addtogroup ml
 *  @{
 */
namespace ml{

template<typename T = double>
class autoencoder
{
public:
    using EigenMat = eigen::MatRowMajor<T>;

    struct layer
    {
        layer() :
            cost_{0.0}
        {

        }
        layer(int input_size, int hidden_size,
              double cost = 0) :
            cost_{cost}
        {
            std::random_device rd;
            std::default_random_engine re(rd());
            std::uniform_real_distribution<T> ur(0, 1);
            ur(re);
            w1_.resize(hidden_size, input_size);
            w2_.resize(input_size, hidden_size);
            b1_ = EigenMat::Zero(hidden_size, 1);
            b2_ = EigenMat::Zero(input_size, 1);

            for(size_t row = 0; row != w1_.rows(); ++row){
                for(size_t col = 0; col != w1_.cols(); ++col){
                    w1_(row, col) = ur(re);
                }
            }
            for(size_t row = 0; row != w2_.rows(); ++row){
                for(size_t col = 0; col != w2_.cols(); ++col){
                    w2_(row, col) = ur(re);
                }
            }

            double const R  = std::sqrt(6) /
                    std::sqrt(hidden_size + input_size + 1);
            w1_ = w1_.array() * 2 * R - R;
            w2_ = w2_.array() * 2 * R - R;

            w1_grad_ = EigenMat::Zero(hidden_size, input_size);
            w2_grad_ = EigenMat::Zero(input_size, hidden_size);
            b1_grad_ = EigenMat::Zero(hidden_size, 1);
            b2_grad_ = EigenMat::Zero(input_size, 1);
        }

        EigenMat w1_;
        EigenMat w2_;
        EigenMat b1_;
        EigenMat b2_;
        EigenMat w1_grad_;
        EigenMat w2_grad_;
        EigenMat b1_grad_;
        EigenMat b2_grad_;
        double cost_;
    };

    explicit autoencoder(cv::AutoBuffer<int> const &hidden_size) :
        reuse_layer_{false}
    {
        set_hidden_layer_size(hidden_size);
    }

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

    EigenMat const& get_last_features() const
    {
        return eactivation_;
    }

    std::vector<layer> const& get_layer() const
    {
        return layers_;
    }

    /**
     * @brief read the train result from the file(xml)
     * @param file the file save the train result
     */
    void read(std::string const &file)
    {
        cv::FileStorage in(file, cv::FileStorage::READ);
        int layer_size = 0;
        in["layer_size"]>>layer_size;
        in["batch_size"]>>params_.batch_size_;
        in["beta_"]>>params_.beta_;
        in["eps_"]>>params_.eps_;
        in["lambda_"]>>params_.lambda_;
        in["lrate_"]>>params_.lrate_;
        in["max_iter_"]>>params_.max_iter_;
        in["sparse_"]>>params_.sparse_;
        params_.hidden_size_.resize(layer_size);
        layers_.clear();
        for(int i = 0; i != layer_size; ++i){
            cv_layer ls;
            auto const Index = std::to_string(i);
            in["w1_" + Index] >> ls.w1_;
            in["w2_" + Index] >> ls.w2_;
            in["w1_grad_" + Index] >> ls.w1_grad_;
            in["w2_grad_" + Index] >> ls.w2_grad_;
            in["b1_" + Index] >> ls.b1_;
            in["b2_" + Index] >> ls.b2_;
            in["b1_grad_" + Index] >> ls.b1_grad_;
            in["b2_grad_" + Index] >> ls.b2_grad_;
            in["hidden_size_" + Index]>>
                                         params_.hidden_size_[i];
            layer el;
            convert(ls, el);
            layers_.emplace_back(el);
        }
        cv::Mat activation;
        in["activation"] >> activation;
        eigen::cv2eigen_cpy(activation, eactivation_);
    }

    /**
     * @brief Set the batch divide parameter, this parameter\n
     * will determine the fraction of the samples will be use\n
     * when finding the cost
     * @param size set up the mini-batch size every time the\n
     * iteration will use
     */
    void set_batch_size(int size)
    {
        params_.batch_size_ = size;
    }

    /**
     * @brief set the beta of autoencoder
     * @param beta the weight of the sparsity penalty term.\n
     * Must be real positive number.The default value is 3
     */
    void set_beta(double beta)
    {
        params_.beta_ = beta;
    }

    /**
         * @brief set the epsillon
         * @param eps The desired accuracy or change in parameters\n
         *  at which the iterative algorithm stops.
         */
    void set_epsillon(double eps){
        params_.eps_ = eps;
    }

    /**
     * @brief set the hidden layers size
     * @param size size of each hidden layers,must bigger\n
     * than zero.The default value is 0
     */
    void set_hidden_layer_size(cv::AutoBuffer<int> const &size)
    {
        params_.hidden_size_ = size;
    }

    /**
     * @brief set the lambda of the regularization term
     * @param lambda the weight of the regularization term.\n
     * Must be real positive number.The default value is 3e-3
     */
    void set_lambda(double lambda)
    {
        params_.lambda_ = lambda;
    }

    /**
     * @brief set the learning rate
     * @param The larger the lrate, the faster we approach the solution,\n
     *  but larger lrate may incurr divergence, must be real\n
     *  positive number.The default value is 2e-2
     */
    void set_learning_rate(double lrate)
    {
        params_.lrate_ = lrate;
    }

    /**
     * @brief set maximum iteration time
     * @param iter the maximum iteration time of the algorithm.\n
     * The default value is 80000
     */
    void set_max_iter(int iter)
    {

        params_.max_iter_ = iter;
    }

    /**
     * @brief set the sparsity penalty
     * @param sparse Constraint of the hidden neuron, the lower it is,\n
     * the sparser the output of the layer would be.The default\n
     * value is 0.1
     */
    void set_sparse(double sparse)
    {
        params_.sparse_ = sparse;
    }

    /**
     * @brief set the trained layer should be reuse or not
     * @param reuse true will reuse the trained layer if exist;else\n
     * the train function will start a new training process
     */
    void set_reuse_layer(bool reuse)
    {
        reuse_layer_ = reuse;
    }

    /**
     * @brief train by sparse autoencoder
     * @param input the input image, type must be double.\n
     * input contains one training example per column
     */

    /*! \brief example.
     *\code
     * ocv::eigen::EigenMat buffer(16*16, 10000);
     * cv::Mat train = ocv::eigen::eigen2cv_ref(buffer);
     * //read_mnist will read the data of mnist into cv::Mat
     * read_mnist(train, "train_0", 10000);
     * train /= 255.0;
     *
     * cv::AutoBuffer<int> hidden_size(2);
     * hidden_size[0] = buffer.cols() * 3 / 4;
     * hidden_size[1] = hidden_size[0];
     * ocv::ml::autoencoder encoder(hidden_size);
     * encoder.set_batch_fraction(20);
     * encoder.train(train);
     * encoder.write("train.xml");
     *\endcode
    */
    template<typename Derived>
    void train(Eigen::MatrixBase<Derived> const &input)
    {
        if(!reuse_layer_){
            layers_.clear();
        }
        std::random_device rd;
        std::default_random_engine re(rd());
        int const Batch = get_batch_size(input.cols());
        int const RandomSize = input.cols() != Batch ?
                    input.cols() - Batch - 1 : 0;
        std::uniform_int_distribution<int>
                uni_int(0, RandomSize);

#ifdef OCV_TEST_AUTOENCODER
        gradient_check();
#endif

        for(size_t i = 0; i < params_.hidden_size_.size(); ++i){
            Eigen::MatrixBase<Derived> const &TmpInput =
                    i == 0 ? input
                           : eactivation_;
            if(!reuse_layer_){
                layer es(TmpInput.rows(), params_.hidden_size_[i]);
                reduce_cost(uni_int, re, Batch, TmpInput, es);
                generate_activation(es, TmpInput,
                                    i==0?true:false);
                layers_.push_back(es);
            }else{
                if(layers_.size() <= i){
                    layers_.emplace_back(static_cast<int>(TmpInput.rows()),
                                         params_.hidden_size_[i]);
                }
                reduce_cost(uni_int, re, Batch, TmpInput, layers_[i]);
                generate_activation(layers_[i], TmpInput,
                                    i==0?true:false);
            }
        }
        act_.clear();
        buffer_.clear();//*/
    }

    /**
     * @brief write the training result into the file(xml)
     * @param file the name of the file
     */
    void write(std::string const &file) const
    {
        cv::FileStorage out(file, cv::FileStorage::WRITE);
        out<<"layer_size"<<static_cast<int>(layers_.size());
        out<<"batch_size"<<params_.batch_size_;
        out<<"beta_"<<params_.beta_;
        out<<"eps_"<<params_.eps_;
        out<<"lambda_"<<params_.lambda_;
        out<<"lrate_"<<params_.lrate_;
        out<<"max_iter_"<<params_.max_iter_;
        out<<"sparse_"<<params_.sparse_;
        for(size_t i = 0; i != layers_.size(); ++i){
            cv_layer ls;
            convert(layers_[i], ls);
            auto const Index = std::to_string(i);
            out<<("w1_" + Index)<<ls.w1_;
            out<<("w2_" + Index)<<ls.w2_;
            out<<("w1_grad_" + Index)<<ls.w1_grad_;
            out<<("w2_grad_" + Index)<<ls.w2_grad_;
            out<<("b1_" + Index)<<ls.b1_;
            out<<("b2_" + Index)<<ls.b2_;
            out<<("b1_grad_" + Index)<<ls.b1_grad_;
            out<<("b2_grad_" + Index)<<ls.b2_grad_;
            out<<("hidden_size_" + Index)<<
                 params_.hidden_size_[i];
        }
        cv::Mat const Activation = eigen::eigen2cv_ref(eactivation_);
        out<<"activation"<<Activation;
    }
private:
    using MatType = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using Mapper = Eigen::Map<MatType, Eigen::Aligned>;
    using MapperConst = Eigen::Map<const MatType, Eigen::Aligned>;

    struct activation
    {
        void clear()
        {
            hidden_.resize(0, 0);
            output_.resize(0, 0);
        }

        EigenMat hidden_;
        EigenMat output_;
    };

    struct buffer
    {
        void clear()
        {
            delta2_.resize(0, 0);
            delta3_.resize(0, 0);
            delta_buffer_.resize(0, 0);
            pj_.resize(0, 0);
            pj_r0_.resize(0, 0);
            pj_r1_.resize(0, 0);
        }

        EigenMat delta2_;
        EigenMat delta3_;
        EigenMat delta_buffer_;
        EigenMat pj_; //the average activation of hidden units
        EigenMat pj_r0_; //same as pj_ expect 0(set to max() of double)
        EigenMat pj_r1_; //same as pj_ expect 1(set to max() of double)
    };

    struct cv_layer
    {
        cv_layer() :
            cost_{0}
        {
        }
        cv_layer(int input_size, int hidden_size,
                 int mat_type,
                 double cost = 0) :
            cost_{cost}
        {
            w1_.create(hidden_size, input_size, mat_type);
            w2_.create(input_size, hidden_size, mat_type);
            b1_.create(hidden_size, 1, mat_type);
            b2_.create(input_size, 1, mat_type);

            generate_random_value<T>(w1_, 0.12);
            generate_random_value<T>(w2_, 0.12);
            generate_random_value<T>(b1_, 0.12);
            generate_random_value<T>(b2_, 0.12);

            w1_grad_ = cv::Mat::zeros(hidden_size, input_size, mat_type);
            w2_grad_ = cv::Mat::zeros(input_size, hidden_size, mat_type);
            b1_grad_ = cv::Mat::zeros(hidden_size, 1, mat_type);
            b2_grad_ = cv::Mat::zeros(input_size, 1, mat_type);
        }

        cv::Mat w1_;
        cv::Mat w2_;
        cv::Mat b1_;
        cv::Mat b2_;
        cv::Mat w1_grad_;
        cv::Mat w2_grad_;
        cv::Mat b1_grad_;
        cv::Mat b2_grad_;
        double cost_;
    };

    struct criteria
    {
        criteria() :
            batch_size_{100},
            beta_{3},
            eps_{1e-8},
            lambda_{3e-3},
            lrate_{2e-2},
            max_iter_{80000},
            sparse_{0.1}
        {

        }

        int batch_size_;
        double beta_;
        double eps_;
        cv::AutoBuffer<int> hidden_size_;
        double lambda_;
        double lrate_; //learning rate
        int max_iter_;
        double sparse_;
    };

    void convert(cv_layer const &input,
                 layer &output) const
    {
        eigen::cv2eigen_cpy(input.b1_, output.b1_);
        eigen::cv2eigen_cpy(input.b2_, output.b2_);
        eigen::cv2eigen_cpy(input.w1_, output.w1_);
        eigen::cv2eigen_cpy(input.w2_, output.w2_);
        eigen::cv2eigen_cpy(input.b1_grad_, output.b1_grad_);
        eigen::cv2eigen_cpy(input.b2_grad_, output.b2_grad_);
        eigen::cv2eigen_cpy(input.w1_grad_, output.w1_grad_);
        eigen::cv2eigen_cpy(input.w2_grad_, output.w2_grad_);
    }
    void convert(layer const &input,
                 cv_layer &output) const
    {
        eigen::eigen2cv_cpy(input.b1_, output.b1_);
        eigen::eigen2cv_cpy(input.b2_, output.b2_);
        eigen::eigen2cv_cpy(input.w1_, output.w1_);
        eigen::eigen2cv_cpy(input.w2_, output.w2_);
        eigen::eigen2cv_cpy(input.b1_grad_, output.b1_grad_);
        eigen::eigen2cv_cpy(input.b2_grad_, output.b2_grad_);
        eigen::eigen2cv_cpy(input.w1_grad_, output.w1_grad_);
        eigen::eigen2cv_cpy(input.w2_grad_, output.w2_grad_);
    }

    template<typename Derived>
    void compute_cost(Eigen::MatrixBase<Derived> const &input,
                      layer &es)
    {
        //std::cout<<&input(0, 0)<<"\n";
        get_last_features(input, es);
        //std::cout<<"get activation\n";
        auto const NSamples = input.cols();
        //square error of back propagation(first half)
        double const SquareError =
                ((act_.output_.array() - input.array()).pow(2.0) / 2.0).
                sum() / NSamples;
        //std::cout<<"square error : "<<SquareError<<"\n";
        // the second part is weight decay part
        double const WeightError =
                ((es.w1_.array() * es.w1_.array()).sum() +
                 (es.w2_.array() * es.w2_.array()).sum()) *
                (params_.lambda_ / 2.0);
        //std::cout<<"weight error : "<<WeightError<<"\n";

        // now calculate pj which is the average activation of hidden units
        buffer_.pj_ = act_.hidden_.rowwise().sum() / NSamples;
        //prevent division by zero
        buffer_.pj_r0_ = (buffer_.pj_.array() != 0.0).
                select(buffer_.pj_, 1000);
        buffer_.pj_r1_ = (buffer_.pj_.array() != 1.0).
                select(buffer_.pj_, -1000);//*/

        //the third part of overall cost function is the sparsity part
        double const Sparse = params_.sparse_;
        //beta * sum(sparse * log[sparse/pj] +
        //           (1 - sparse) * log[(1-sparse)/(1-pj)])
        double const SparseError =
                ( (Sparse * (Sparse / (buffer_.pj_r0_.array())).log()) +
                  (1.0-Sparse)*((1.0-Sparse)/(1.0-buffer_.pj_r1_.array())).log()).sum() *
                params_.beta_;
        //std::cout<<"Sparse error : "<<SparseError<<"\n";
        es.cost_ = SquareError + WeightError + SparseError;//*/
    }

    template<typename Derived>
    void compute_gradient(Eigen::MatrixBase<Derived> const &input,
                          layer &es)
    {
        auto const NSamples = input.cols();
        buffer_.delta3_ =
                ((act_.output_.array() - input.array()) / NSamples) *
                ((1.0 - act_.output_.array()) * act_.output_.array());
        //std::cout<<buffer_.delta3_<<"\n\n";
        es.w2_grad_.noalias() = buffer_.delta3_*act_.hidden_.transpose();
        es.w2_grad_ = (es.w2_grad_.array()) +
                params_.lambda_ * es.w2_.array();
        es.b2_grad_ = buffer_.delta3_.rowwise().sum();

        get_delta_2(buffer_.delta3_, es, NSamples);
        es.w1_grad_.noalias() = buffer_.delta2_*input.transpose();
        es.w1_grad_ = (es.w1_grad_.array()) +
                params_.lambda_ * es.w1_.array();
        es.b1_grad_ = buffer_.delta2_.rowwise().sum();
    }

    template<typename Derived>
    void generate_activation(layer const &ls,
                             Eigen::MatrixBase<Derived> const &temp_input,
                             bool no_overlap = true)
    {
#ifdef OCV_MEASURE_TIME
        auto const TGen =
                time::measure<>::duration([&]()
        { generate_activation_impl(ls, temp_input,
                                   no_overlap); });
        std::cout<<"time of generate last layer activation : "<<TGen.count()<<"\n\n";
#else
        generate_activation_impl(ls, temp_input, no_overlap);
#endif
    }

    template<typename Derived>
    void generate_activation_impl(layer const &ls,
                                  Eigen::MatrixBase<Derived> const &temp_input,
                                  bool no_overlap = true)
    {
        forward_propagation(temp_input, ls.w1_,
                            ls.b1_, eactivation_,
                            no_overlap);
    }

    template<typename Derived>
    void get_last_features(Eigen::MatrixBase<Derived> const &input,
                        layer &es)
    {
        forward_propagation(input, es.w1_, es.b1_, act_.hidden_);
        forward_propagation(act_.hidden_, es.w2_, es.b2_, act_.output_);
    }
    int get_batch_size(int sample_size) const
    {
        return std::min(params_.batch_size_, sample_size);
    }

    template<typename Derived>
    void get_delta_2(Eigen::MatrixBase<Derived> const &delta_3,
                     layer const &es,
                     size_t sample_size)
    {
        buffer_.delta_buffer_ =
                (params_.beta_ / sample_size) *
                (-params_.sparse_/(buffer_.pj_r0_.array()) +
                 (1.0-params_.sparse_)/(1.0-buffer_.pj_r1_.array()));

        Mapper Map(buffer_.delta_buffer_.data(),
                   buffer_.delta_buffer_.size());
        buffer_.delta2_.noalias() = es.w2_.transpose() * delta_3;
        buffer_.delta2_.colwise() += Map;
        buffer_.delta2_ =
                buffer_.delta2_.array() *
                ((1.0 - act_.hidden_.array()) * act_.hidden_.array());
    }

    void gradient_check()
    {
        EigenMat const Input = EigenMat::Random(8, 2);
        layer es(Input.rows(), Input.rows() / 2);
        layer es_copy = es;
        gradient_checking gc;
        auto func = [&](EigenMat &theta)->T
        {
            es.w1_.swap(theta);
            compute_cost(Input, es);
            auto const Cost = es.cost_;
            es.w1_.swap(theta);

            return Cost;
        };
        EigenMat const Gradient =
                gc.compute_gradient(es.w1_,
                                    func);

        compute_cost(Input, es_copy);
        compute_gradient(Input, es_copy);

        std::cout<<std::boolalpha<<"pass : "<<
                   gc.compare_gradient(Gradient, es_copy.w1_grad_)<<"\n";
    }

    template<typename Derived>
    void reduce_cost(std::uniform_int_distribution<int> const &uni_int,
                     std::default_random_engine &re,
                     int batch, Eigen::MatrixBase<Derived> const &input,
                     layer &ls)
    {
        double last_cost = 0.0;
        auto const LRate = params_.lrate_;
#ifndef  OCV_MEASURE_TIME
        for(int j = 0; j != params_.max_iter_; ++j){
            int const X = uni_int(re);
            auto const &Temp = input.block(0, X,
                                           input.rows(), batch);
            compute_cost(Temp, ls);

#ifdef OCV_PRINT_COST
            std::cout<<j<<" : cost : "<<ls.cost_
                    <<", random : "<<X<<"\n";
#endif

            if(std::abs(last_cost - ls.cost_) < params_.eps_ ||
                    ls.cost_ <= 0.0){
                break;
            }

            compute_gradient(Temp, ls);
            //std::cout<<ls.w1_<<"\n\n";

            last_cost = ls.cost_;
            update_weight(ls);
            //std::cout<<ls.b2_grad_<<"\n\n";
        }
#else
        double t_cost = 0;
        double t_gra = 0;
        double t_update = 0;
        int iter_time = 1;
        for(int j = 0; j != params_.max_iter_; ++j){
            int const X = uni_int(re);
            auto const &Temp = input.block(0, X,
                                           input.rows(), batch);
            t_cost +=
                    time::measure<>::execution([&]()
            { compute_cost(Temp, ls); });

#ifdef OCV_PRINT_COST
            std::cout<<j<<" : cost : "<<ls.cost_
                    <<", random : "<<X<<"\n";
#endif

            if(std::abs(last_cost - ls.cost_) < params_.eps_ ||
                    ls.cost_ <= 0.0){
                break;
            }

            t_gra +=
                    time::measure<>::execution([&]()
            { compute_gradient(Temp, ls); });
            ++iter_time;

            last_cost = ls.cost_;
            t_update +=
                    time::measure<>::execution([&]()
            { update_weight(ls); });
        }

        std::cout<<"total encoder cost time : "<<t_cost<<"\n";
        std::cout<<"total gradient cost time : "<<t_gra<<"\n";
        std::cout<<"total update time : "<<t_gra<<"\n";
        std::cout<<"total time of update weight and bias : "<<t_update<<"\n";
        std::cout<<"average encoder cost time : "<<t_cost / iter_time<<"\n";
        std::cout<<"average gradient cost time : "<<t_gra / iter_time<<"\n";
        std::cout<<"average update time : "<<t_gra / iter_time<<"\n";
        std::cout<<"average time of update weight and bias : "<<t_update / iter_time<<"\n";
#endif
        params_.lrate_ = LRate;
    }

    void update_weight(layer &ls)
    {
        update_weight(ls.w1_grad_, ls.w1_);
        update_weight(ls.w2_grad_, ls.w2_);
        update_weight(ls.b1_grad_, ls.b1_);
        update_weight(ls.b2_grad_, ls.b2_);
    }

    template<typename Derived>
    void update_weight(Eigen::MatrixBase<Derived> const &gradient,
                       Eigen::MatrixBase<Derived> &weight)
    {
        weight = weight.array() -
                params_.lrate_ * gradient.array();
    }

    activation act_;
    buffer buffer_;
    criteria params_;
    EigenMat eactivation_;
    std::vector<layer> layers_;
    bool reuse_layer_;
};

} /*! @} End of Doxygen Groups*/

} /*! @} End of Doxygen Groups*/

#endif // AUTOENCODER_H


///////////////////////////////////////////////////////////////////////////////
// weighted_kurtosis.hpp
//
//  Copyright 2006 Olivier Gygi, Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_KURTOSIS_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_KURTOSIS_HPP_EAN_28_10_2005

#include <limits>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // weighted_kurtosis_impl
    /**
        @brief Kurtosis estimation for weighted samples

        The kurtosis of a sample distribution is defined as the ratio of the 4th central moment and the square of the 2nd central
        moment (the variance) of the samples, minus 3. The term \f$ -3 \f$ is added in order to ensure that the normal distribution
        has zero kurtosis. The kurtosis can also be expressed by the simple moments:

        \f[
            \hat{g}_2 =
                \frac
                {\widehat{m}_n^{(4)}-4\widehat{m}_n^{(3)}\hat{\mu}_n+6\widehat{m}_n^{(2)}\hat{\mu}_n^2-3\hat{\mu}_n^4}
                {\left(\widehat{m}_n^{(2)} - \hat{\mu}_n^{2}\right)^2} - 3,
        \f]

        where \f$ \widehat{m}_n^{(i)} \f$ are the \f$ i \f$-th moment and \f$ \hat{\mu}_n \f$ the mean (first moment) of the
        \f$ n \f$ samples.

        The kurtosis estimator for weighted samples is formally identical to the estimator for unweighted samples, except that
        the weighted counterparts of all measures it depends on are to be taken.
    */
    template<typename Sample, typename Weight>
    struct weighted_kurtosis_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, weighted_sample>::result_type result_type;

        weighted_kurtosis_impl(dont_care)
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::average(
                        accumulators::weighted_moment<4>(args)
                        - 4. * accumulators::weighted_moment<3>(args) * weighted_mean(args)
                        + 6. * accumulators::weighted_moment<2>(args) * weighted_mean(args) * weighted_mean(args)
                        - 3. * weighted_mean(args) * weighted_mean(args) * weighted_mean(args) * weighted_mean(args)
                      , ( accumulators::weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
                        * ( accumulators::weighted_moment<2>(args) - weighted_mean(args) * weighted_mean(args) )
                   ) - 3.;
        }
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_kurtosis
//
namespace tag
{
    struct weighted_kurtosis
      : depends_on<weighted_mean, weighted_moment<2>, weighted_moment<3>, weighted_moment<4> >
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_kurtosis_impl<mpl::_1, mpl::_2> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_kurtosis
//
namespace extract
{
    extractor<tag::weighted_kurtosis> const weighted_kurtosis = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_kurtosis)
}

using extract::weighted_kurtosis;

}} // namespace boost::accumulators

#endif


// Copyright 2017 Daniel James.
// 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)

// clang-format off
#include "../helpers/prefix.hpp"
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include "../helpers/postfix.hpp"
// clang-format on

#include "../helpers/test.hpp"
#include <map>

// Pretty inefficient, but the test is fast enough.
// Might be too slow if we had larger primes?
bool is_prime(std::size_t x)
{
  if (x == 2) {
    return true;
  } else if (x == 1 || x % 2 == 0) {
    return false;
  } else {
    // y*y <= x had rounding errors, so instead use y <= (x/y).
    for (std::size_t y = 3; y <= (x / y); y += 2) {
      if (x % y == 0) {
        return false;
        break;
      }
    }

    return true;
  }
}

void test_next_prime(std::size_t value)
{
  std::size_t x = boost::unordered::detail::next_prime(value);
  BOOST_TEST(is_prime(x));
  BOOST_TEST(x >= value);
}

void test_prev_prime(std::size_t value)
{
  std::size_t x = boost::unordered::detail::prev_prime(value);
  BOOST_TEST(is_prime(x));
  BOOST_TEST(x <= value);
  if (x > value) {
    BOOST_LIGHTWEIGHT_TEST_OSTREAM << x << "," << value << std::endl;
  }
}

UNORDERED_AUTO_TEST (next_prime_test) {
  BOOST_TEST(!is_prime(0));
  BOOST_TEST(!is_prime(1));
  BOOST_TEST(is_prime(2));
  BOOST_TEST(is_prime(3));
  BOOST_TEST(is_prime(13));
  BOOST_TEST(!is_prime(4));
  BOOST_TEST(!is_prime(100));

  BOOST_TEST(boost::unordered::detail::next_prime(0) > 0);

  // test_prev_prime doesn't work for values less than 17.
  // Which should be okay, unless an allocator has a really tiny
  // max_size?
  const std::size_t min_prime = 17;

  // test_next_prime doesn't work for values greater than this,
  // which might be a problem if you've got terrabytes of memory?
  // I seriously doubt the container would work well at such sizes
  // regardless.
  const std::size_t max_prime = 4294967291ul;

  std::size_t i;

  BOOST_TEST(is_prime(min_prime));
  BOOST_TEST(is_prime(max_prime));

  for (i = 0; i < 10000; ++i) {
    if (i < min_prime) {
      BOOST_TEST(boost::unordered::detail::prev_prime(i) == min_prime);
    } else {
      test_prev_prime(i);
    }
    test_next_prime(i);
  }

  std::size_t last = i - 1;
  for (; i > last; last = i, i += i / 5) {
    if (i > max_prime) {
      BOOST_TEST(boost::unordered::detail::next_prime(i) == max_prime);
    } else {
      test_next_prime(i);
    }
    test_prev_prime(i);
  }
}

RUN_TESTS()


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <boost/math/special_functions/expint.hpp>
#include <eve/function/exp_int.hpp>
#include <eve/function/exp.hpp>
#include <eve/function/oneminus.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>

TTS_CASE_TPL("Check eve::exp_int return type", EVE_TYPE)
{
  TTS_EXPR_IS(eve::exp_int(T(0), T(0)), T);
  TTS_EXPR_IS(eve::exp_int(0, T(0)), T);
}

TTS_CASE_TPL("Check eve::exp_int behavior", EVE_TYPE)
{
  auto eve__exp_int   =  [](auto x, auto y) { return eve::exp_int(x, y); };
  auto boost__exp_int =  [](auto x, auto y) { return boost::math::expint(x, y); };

//   if constexpr( eve::platform::supports_invalids )
//   {
//     TTS_IEEE_EQUAL(eve__exp_int(T(1), eve::nan(eve::as<T>()))  , eve::nan(eve::as<T>()) );
//     TTS_IEEE_EQUAL(eve__exp_int(T(1), eve::inf(eve::as<T>()))   , T(0) );
//   }

  for(int i=1; i < 4 ; ++i)
  {
    TTS_ULP_EQUAL(eve__exp_int(T(i), T(0))  , eve::rec(T(i-1)), 0.5);
    TTS_ULP_EQUAL(eve__exp_int(T(i), T(0.5)), T(boost__exp_int(i, 0.5)), 2.0);
    TTS_ULP_EQUAL(eve__exp_int(T(i), T(1))  , T(boost__exp_int(i, 1.0)), 4.0);
    TTS_ULP_EQUAL(eve__exp_int(T(i), T(10)) , T(boost__exp_int(i, 10.0)), 0.5);
  }
  for(int i=1; i < 4 ; ++i)
  {
    TTS_ULP_EQUAL(eve__exp_int(i, T(0))  , eve::rec(T(i-1)), 0.5);
    TTS_ULP_EQUAL(eve__exp_int(i, T(0.5)), T(boost__exp_int(i, 0.5)), 2.0);
    TTS_ULP_EQUAL(eve__exp_int(i, T(1))  , T(boost__exp_int(i, 1.0)), 4.0);
    TTS_ULP_EQUAL(eve__exp_int(i, T(10)) , T(boost__exp_int(i, 10.0)), 0.5);
  }
  using elt_t =  eve::element_type_t<T>;

  TTS_ULP_EQUAL(eve__exp_int(elt_t(2.0), elt_t(0.5)), (boost__exp_int(elt_t(2), elt_t(0.5))), 2.0);
  TTS_ULP_EQUAL(eve__exp_int(elt_t(6000), elt_t(0.5)), (boost__exp_int(elt_t(6000), elt_t(0.5))), 3.0);
}


/**
 * @file debuggingfem_main.cc
 * @brief NPDE homework ParametricElementMatrices code
 * @author Simon Meierhans
 * @date 27/03/2019
 * @copyright Developed at ETH Zurich
 */

#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/refinement/refinement.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <memory>
#include <vector>

#include "locallaplaceqfe.h"
#include "qfeinterpolator.h"
#include "qfeprovidertester.h"

using size_type = lf::base::size_type;

int main() {
  // read mesh
  auto mesh_factory = std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2);
  lf::io::GmshReader reader(std::move(mesh_factory),
                            CURRENT_SOURCE_DIR "/../meshes/square_64.msh");
  auto mesh = reader.mesh();

  // refine mesh
  const int reflevels = 4;
  std::shared_ptr<lf::refinement::MeshHierarchy> multi_mesh_p =
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh, reflevels);
  lf::refinement::MeshHierarchy &multi_mesh{*multi_mesh_p};
  lf::base::size_type L = multi_mesh.NumLevels();

  // vector holding pointers to the different element matrix providers
  std::vector<std::unique_ptr<DebuggingFEM::EntityMatrixProvider>>
      element_matrix_provider;
  element_matrix_provider.emplace_back(
      std::make_unique<DebuggingFEM::LocalLaplaceQFE1>());
  element_matrix_provider.emplace_back(
      std::make_unique<DebuggingFEM::LocalLaplaceQFE2>());
  element_matrix_provider.emplace_back(
      std::make_unique<DebuggingFEM::LocalLaplaceQFE3>());
  const int num_emp = element_matrix_provider.size();

  // vectors accumulating the computed errors
  Eigen::MatrixXd H1SMerr(L, num_emp);
  Eigen::VectorXi N(L);

  for (int level = 0; level < L; ++level) {
    // set up fespace and dof handler for the mesh at the current level
    auto mesh_p = multi_mesh.getMesh(level);
    lf::uscalfe::FeSpaceLagrangeO2<double> fespace(mesh_p);
    const auto &dofh = fespace.LocGlobMap();

    // Dimension of finite element space
    N[level] = dofh.NumDofs();

    // compute error for each element matrix provider
    for (int i = 0; i < num_emp; ++i) {
      // function to interpolateOntoQuadFE
      auto f = [](const Eigen::VectorXd &x) -> double {
        return std::exp(x(1) * x(1) + x(0) * x(0));
      };
      double energy = 0.0;
      // Matrix in triplet format holding Galerkin for LocalLaplaceQFEX matrix,
      // zero initially.
      DebuggingFEM::QFEProviderTester qfe_provider_tester(
          dofh, *element_matrix_provider[i]);
      // compute the energy and error
      energy = qfe_provider_tester.energyOfInterpolant(f);
      H1SMerr(level, i) = std::abs(23.76088 - energy);
    }
  }

  // Tabular output of the results
  std::cout << std::left << std::setw(10) << "N" << std::setw(20)
            << "Assember 1" << std::setw(20) << "Assembler 2" << std::setw(20)
            << "Assembler 3" << std::endl;
  for (int level = 0; level < L; ++level) {
    std::cout << std::left << std::setw(10) << N[level] << std::setw(20)
              << H1SMerr(level, 0) << std::setw(20) << H1SMerr(level, 1)
              << std::setw(20) << H1SMerr(level, 2) << std::endl;
  }

  // Write .csv file and plot it by a python script
  Eigen::MatrixXd data(L, num_emp + 1);
  data.col(0) = N.cast<double>();
  data.rightCols(num_emp) = H1SMerr;
  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream error_file;
  error_file.open("error.csv");
  error_file << data.format(CSVFormat) << std::endl;
  error_file.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/error.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_error.py " CURRENT_BINARY_DIR
              "/error.csv " CURRENT_BINARY_DIR "/error.eps");

  return 0;
}


#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/simple_hierarchical_clustering.hpp>
#include <common_robotics_utilities/simple_kmeans_clustering.hpp>

int main(int argc, char** argv)
{
  const size_t num_points
      = (argc >= 2) ? static_cast<size_t>(atoi(argv[1])) : 1000;
  std::cout << "Generating " << num_points << " points..." << std::endl;
  const int64_t seed = 42;
  std::mt19937_64 prng(seed);
  std::uniform_real_distribution<double> dist(0.0, 10.0);
  std::vector<Eigen::VectorXd> random_points(num_points);
  std::vector<size_t> indices(num_points);
  for (size_t idx = 0; idx < num_points; idx++)
  {
    const double x = dist(prng);
    const double y = dist(prng);
    Eigen::VectorXd random_point(3);
    random_point << x, y, 0.0;
    random_points[idx] = random_point;
    indices[idx] = idx;
  }
  const std::vector<bool> use_parallel_options = {false, true};
  for (const bool use_parallel : use_parallel_options)
  {
    std::cout << "Complete-link hierarchical clustering " << num_points
              << " points..." << std::endl;
    std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)>
        distance_fn = [] (const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
    {
      return common_robotics_utilities::math::Distance(v1, v2);
    };
    const Eigen::MatrixXd distance_matrix
        = common_robotics_utilities::math
            ::BuildPairwiseDistanceMatrixParallel(random_points, distance_fn);
    const std::vector<std::vector<size_t>> hierarchical_index_clusters
        = common_robotics_utilities::simple_hierarchical_clustering
            ::ClusterWithDistanceMatrix<int64_t>(
                indices, distance_matrix, 1.0,
                common_robotics_utilities::simple_hierarchical_clustering
                    ::ClusterStrategy::COMPLETE_LINK, use_parallel).first;
    std::vector<Eigen::VectorXd> hierarchical_clustered_points = random_points;
    for (size_t cluster_idx = 0;
         cluster_idx < hierarchical_index_clusters.size(); cluster_idx++)
    {
      const std::vector<size_t>& current_cluster
          = hierarchical_index_clusters[cluster_idx];
      const double cluster_num = 1.0 + (double)cluster_idx;
      for (size_t element_idx = 0; element_idx < current_cluster.size();
           element_idx++)
      {
        const size_t index = current_cluster[element_idx];
        hierarchical_clustered_points.at(index)(2) = cluster_num;
      }
    }
    std::cout << "K-means clustering " << num_points << " points..."
              << std::endl;
    std::function<Eigen::VectorXd(const std::vector<Eigen::VectorXd>&)>
        average_fn = [] (const std::vector<Eigen::VectorXd>& cluster)
    {
      return common_robotics_utilities::math::AverageEigenVectorXd(cluster);
    };
    const std::vector<int32_t> kmeans_labels
        = common_robotics_utilities::simple_kmeans_clustering::Cluster(
            random_points, distance_fn, average_fn, 50, 42, true, use_parallel);
    std::vector<Eigen::VectorXd> kmeans_clustered_points = random_points;
    for (size_t idx = 0; idx < kmeans_clustered_points.size(); idx++)
    {
      kmeans_clustered_points.at(idx)(2) = kmeans_labels.at(idx);
    }
    std::cout << "Saving to CSV..." << std::endl;
    const std::string hierarchical_log_file_name
        = (use_parallel) ? "/tmp/test_parallel_hierarchical_clustering.csv"
                         : "/tmp/test_hierarchical_clustering.csv";
    std::ofstream hierarchical_log_file(
        hierarchical_log_file_name, std::ios_base::out);
    if (!hierarchical_log_file.is_open())
    {
      std::cerr << "\x1b[31;1m Unable to create folder/file to log to: "
                << hierarchical_log_file_name << "\x1b[0m \n";
      throw std::invalid_argument("Log filename must be write-openable");
    }
    for (size_t idx = 0; idx < num_points; idx++)
    {
      const Eigen::VectorXd& point = hierarchical_clustered_points.at(idx);
      hierarchical_log_file << point(0) << "," << point(1) << "," << point(2)
                            << std::endl;
    }
    hierarchical_log_file.close();
    const std::string kmeans_log_file_name
        = (use_parallel) ? "/tmp/test_parallel_kmeans_clustering.csv"
                         : "/tmp/test_kmeans_clustering.csv";
    std::ofstream kmeans_log_file(kmeans_log_file_name, std::ios_base::out);
    if (!kmeans_log_file.is_open())
    {
      std::cerr << "\x1b[31;1m Unable to create folder/file to log to: "
                << kmeans_log_file_name << "\x1b[0m \n";
      throw std::invalid_argument("Log filename must be write-openable");
    }
    for (size_t idx = 0; idx < num_points; idx++)
    {
      const Eigen::VectorXd& point = kmeans_clustered_points.at(idx);
      kmeans_log_file << point(0) << "," << point(1) << "," << point(2)
                      << std::endl;
    }
    kmeans_log_file.close();
  }
  std::cout << "Done saving, you can plot as a 3d scatterplot to see clustering"
            << std::endl;
  return 0;
}


#include "triangle.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <complex>
namespace
{
    ComplexType validate(PASTNode astnode, ParsersHelper& ph, const std::string& fnname)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error(fnname+" can only have one parameter");
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex)
          throw std::runtime_error("The argument of "+fnname+" must be complex");
        return boost::get<ComplexType>(secondCh->token.info).toinexact();
    }
}

namespace HT
{
    void sin(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "sin");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::sin(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void cos(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "cos");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::cos(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void tan(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "tan");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::tan(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void asin(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "asin");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::asin(a);
        if (a.imag() == 0.0 && a.real()>1.0) ans = std::complex<long double>(ans.real(), -ans.imag());
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void acos(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "acos");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        LOG("arg of acos:"<<a)
        auto ans = std::acos(a);
        LOG("acos="<<ans)
        if (a.imag() == 0.0 && a.real()>1.0) ans = std::complex<long double>(ans.real(), -ans.imag());
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }
    void atan(PASTNode astnode, ParsersHelper& ph)
    {

        auto cast = validate(astnode, ph, "atan");
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        auto a = std::complex<long double>(cast.getRealD(), cast.getImagD());
        auto ans = std::atan(a);
        astnode->token.info = ComplexType(
                    ans.real(),
                    ans.imag()
                    );
        astnode->remove();
    }

}


#include <ctime>
#include <gtest/gtest.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include "slick/math/se3.h"
#include "slick/test/unittest/util.h"

using namespace std;
using namespace Eigen;
using namespace slick;

// constructors tests ==========================================================
template <typename T>
void DefaultConstructor_Test() {
  // for double precision
  SE3Group<T> trans;
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 2>::Identity(), trans.get_rotation().get_matrix(),
                     Gap<T>());
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 1>::Zero(), trans.get_translation(),
                     Gap<T>());
}

TEST(SE3Test, DefaultConstructor) {
  DefaultConstructor_Test<double>();
  DefaultConstructor_Test<float>();
}

template <typename T>
void FromSO3VectorConstructor_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  aaxis *= angle;
  SO3Group<T> rot(aaxis);
  Eigen::Matrix<T, 3, 1> t = Eigen::Matrix<T, 3, 1>::Random();
  SE3Group<T> trans(rot, t);

  EXPECT_MATRIX_NEAR(rot.get_matrix(), trans.get_rotation().get_matrix(),
                     Gap<T>());
  EXPECT_MATRIX_NEAR(t, trans.get_translation(),
                     Gap<T>());
}

TEST(SE3Test, FromSO3VectorConstructor) {
  FromSO3VectorConstructor_Test<double>();
  FromSO3VectorConstructor_Test<float>();
}

template <typename T>
void FromVectorConstructor_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  aaxis *= angle;
  SO3Group<T> rot(aaxis);

  Eigen::Matrix<T, 6, 1> v;
  v.segment(0, 3) = Eigen::Matrix<T, 3, 1>::Random();
  v.segment(3, 3) = aaxis;
  SE3Group<T> trans(v);

  EXPECT_MATRIX_NEAR(rot.get_matrix(), trans.get_rotation().get_matrix(),
                     Gap<T>());
}

TEST(SE3Test, FromVectorConstructor) {
  FromVectorConstructor_Test<double>();
  FromVectorConstructor_Test<float>();
}

// template <typename T>
// void FromListInitializerConstructor_Test() {
//   SE3Group<T> rot{1, 0, 0, 1};
//   EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 2>::Identity(), rot.get_matrix(),
//                      Gap<T>());

//   EXPECT_NEAR(0.0, rot.ln(), Gap<T>());
// }

// TEST(SE3Test, FromListInitializerConstructor) {
//   FromListInitializerConstructor_Test<double>();
//   FromListInitializerConstructor_Test<float>();
// }

// se2 specific functions ======================================================
template <typename T>
void Ln_Test() {
  Eigen::Matrix<T, 3, 1> aaxis = Eigen::Matrix<T, 3, 1>::Random();
  aaxis.normalize();
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand()) / T(RAND_MAX) * 2 * M_PI;
  aaxis *= angle;
  SO3Group<T> rot(aaxis);
  Eigen::Matrix<T, 3, 1> t = Eigen::Matrix<T, 3, 1>::Random();
  SE3Group<T> trans(rot, t);

  Eigen::Matrix<T, 3, 1> est_axis = trans.ln().segment(3, 3);
  T d = est_axis.dot(aaxis);
  if (d < 0) {
    auto a = -est_axis.norm() + 2 * M_PI;
    est_axis = -est_axis.normalized() * a;
  }
  EXPECT_MATRIX_NEAR(aaxis, est_axis, Gap<T>());
}

TEST(SE3Test, Ln) {
  Ln_Test<double>();
  Ln_Test<float>();
}

template <typename T>
void Inverse_Test() {
  Eigen::Matrix<T, 6, 1> v = Eigen::Matrix<T, 6, 1>::Random();
  SE3Group<T> trans(v);
  Eigen::Matrix<T, 3, 3> m_inv = trans.get_rotation().get_matrix().inverse();
  EXPECT_MATRIX_NEAR(m_inv, trans.inverse().get_rotation().get_matrix(), Gap<T>());
  // EXPECT_MATRIX_NEAR(-t, trans.inverse().get_translation(), Gap<T>());
}

TEST(SE3Test, Inverse) {
  Inverse_Test<double>();
  Inverse_Test<float>();
}

template <typename T>
void SE3RightHandMulOperator_Test() {
  Eigen::Matrix<T, 6, 1> v = Eigen::Matrix<T, 6, 1>::Random();
  SE3Group<T> trans1(v);
  Eigen::Matrix<T, 4, 4> mtrans1 = Eigen::Matrix<T, 4, 4>::Identity();
  mtrans1.block(0, 0, 3, 4) = trans1.get_matrix();

  v = Eigen::Matrix<T, 6, 1>::Random();
  SE3Group<T> trans2(v);
  Eigen::Matrix<T, 4, 4> mtrans2 = Eigen::Matrix<T, 4, 4>::Identity();
  mtrans2.block(0, 0, 3, 4) = trans2.get_matrix();

  EXPECT_MATRIX_NEAR((mtrans1 * mtrans2).block(0, 0, 3, 4),
    (trans1 * trans2).get_matrix(), Gap<T>());
}

TEST(SE3Test, SE3RightHandMulOperator) {
  SE3RightHandMulOperator_Test<double>();
  SE3RightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixRightHandMulOperator_Test() {
  Eigen::Matrix<T, 6, 1> v = Eigen::Matrix<T, 6, 1>::Random();
  SE3Group<T> trans(v);
  Eigen::Matrix<T, 4, 4> mtrans = Eigen::Matrix<T, 4, 4>::Identity();
  mtrans.block(0, 0, 3, 4) = trans.get_matrix();

  Eigen::Matrix<T, 4, 4> mrand = Eigen::Matrix<T, 4, 4>::Random();
  EXPECT_MATRIX_NEAR(mtrans * mrand, trans * mrand, Gap<T>());
}

TEST(SE3Test, MatrixRightHandMulOperator) {
  MatrixRightHandMulOperator_Test<double>();
  MatrixRightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixLeftHandMulOperator_Test() {
  Eigen::Matrix<T, 6, 1> v = Eigen::Matrix<T, 6, 1>::Random();
  SE3Group<T> trans(v);
  Eigen::Matrix<T, 4, 4> mtrans = Eigen::Matrix<T, 4, 4>::Identity();
  mtrans.block(0, 0, 3, 4) = trans.get_matrix();

  Eigen::Matrix<T, 4, 4> mrand = Eigen::Matrix<T, 4, 4>::Random();
  EXPECT_MATRIX_NEAR(mrand*mtrans , mrand*trans, Gap<T>());
}

TEST(SE3Test, MatrixLeftHandMulOperator) {
  MatrixLeftHandMulOperator_Test<double>();
  MatrixLeftHandMulOperator_Test<float>();
}

int main(int argc, char** argv) {
  std::vector<char*> vars(argc+1);
  for (int i = 0; i < argc; ++i)
    vars[i] = argv[i];
  char ca[50];
  sprintf(ca, "--gtest_repeat=%d", NSAMPLES);
  vars[argc] = ca;
  argc++;
  ::testing::InitGoogleTest(&argc, &vars.front());
  return RUN_ALL_TESTS();
}


/*******************************************************************************
*
*  Filename    : parameter_arithmatics.cc
*  Description : Testing the arithmatics of paramters
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "ManagerUtils/Maths/interface/Parameter.hpp"
#include <iostream>
#include <vector>

#include <boost/format.hpp>
using namespace std;
using namespace mgr;

void
AddTest( const vector<Parameter>& list )
{
    for( size_t i = 0; i < list.size(); ++i ){
        cout << list[ i ].CentralValue();

        if( i != list.size() - 1 ){
            cout << "+";
        }
        else{
            cout << " & ";
        }
    }

    const Parameter sum = SumUncorrelated( list );
    cout << sum.CentralValue() << "&" << sum.AbsUpperError() << "&" << sum.AbsLowerError() << endl;
}

void
ProdTest( const vector<double> chain )
{
    Parameter original  = Poisson::Minos( chain.front() );
    Parameter finalpass = Poisson::Minos( chain.back() );
    vector<Parameter> prodlist;
    prodlist.push_back( original );
    cout << chain.front() << flush;

    for( size_t i = 0; i < chain.size() - 1; ++i ){
        cout << boost::format( "\\times\\frac{%lg}{%lg}" ) % chain[ i + 1 ] % chain[ i ] << flush;
        prodlist.push_back( Efficiency::Minos( chain[ i + 1 ], chain[ i ] ) );
    }

    cout << "&" << flush;
    const Parameter product = ProdUncorrelated( prodlist );
    cout << product.CentralValue() << "&" << product.AbsUpperError() << "&" << product.AbsLowerError() << endl;
}

int
main( int argc, char const* argv[] )
{
    cout << ">>>> Addition testing: Sum to 100" << endl;
    {
        Parameter a = Poisson::Minos( 10 );
        Parameter b = Poisson::Minos( 20 );
        Parameter c = Poisson::Minos( 30 );
        Parameter d = Poisson::Minos( 40 );
        Parameter e = Poisson::Minos( 50 );
        Parameter f = Poisson::Minos( 60 );
        Parameter g = Poisson::Minos( 70 );
        Parameter h = Poisson::Minos( 80 );
        Parameter i = Poisson::Minos( 90 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 100 ), 4 ) << endl;
        AddTest( { e, e } );
        AddTest( { d, f } );
        AddTest( { c, g } );
        AddTest( { b, h } );
        AddTest( { a, i } );
        AddTest( { a, c, c, c } );
        AddTest( { b, b, b, b, b } );
        AddTest( { a, a, a, a, a, a, a, a, a, a } );
        cout << endl;
    }
    cout << ">>>> Addition testing: Sum to 1000" << endl;
    {
        Parameter a = Poisson::Minos( 100 );
        Parameter b = Poisson::Minos( 200 );
        Parameter c = Poisson::Minos( 300 );
        Parameter d = Poisson::Minos( 400 );
        Parameter e = Poisson::Minos( 500 );
        Parameter f = Poisson::Minos( 600 );
        Parameter g = Poisson::Minos( 700 );
        Parameter h = Poisson::Minos( 800 );
        Parameter i = Poisson::Minos( 900 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 1000 ), 4 ) << endl;
        AddTest( { e, e } );
        AddTest( { d, f } );
        AddTest( { c, g } );
        AddTest( { b, h } );
        AddTest( { a, i } );
        AddTest( { a, c, c, c } );
        AddTest( { b, b, b, b, b } );
        AddTest( { a, a, a, a, a, a, a, a, a, a } );
        cout << endl;
    }
    cout << ">>>> Addition testing: Partial addition to 100" << endl;
    {
        Parameter a = Poisson::Minos( 20 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 100 ), 4 ) << endl;
        cout << "(20+20+20+20+20) & " << FloatingPoint( Sum( a, a, a, a, a ), 4 ) << endl;
        cout << "(20+20)+(20+20+20) & " << FloatingPoint( Sum( Sum( a, a ), Sum( a, a, a ) ), 4 ) << endl;
        cout << "(20+20)+(20+20)+20 & " << FloatingPoint( Sum( Sum( a, a ), Sum( a, a ), a ), 4 ) << endl;
        cout << "(((20+20)+20)+20)+20 )& " << FloatingPoint( Sum( Sum( Sum( Sum( a, a ), a ), a ), a ), 4 ) << endl;
        cout << endl;
    }
    cout << ">>>> Product testing: Product to 50" << endl;
    {
        cout << "Direct value : " << FloatingPoint( Poisson::Minos( 50 ), 4 ) << endl;
        ProdTest( { 100, 50 } );
        ProdTest( { 100, 75, 50 } );
        ProdTest( { 100, 75, 60, 50 } );
        ProdTest( { 200, 100, 75, 60, 50 } );
        cout << endl;
    }
    cout << ">>> Product test: Partial product " << endl;
    {
        Parameter a  = Poisson::Minos( 200 );
        Parameter e1 = Efficiency::Minos( 100, 200 );
        Parameter e2 = Efficiency::Minos( 75, 100 );
        Parameter e3 = Efficiency::Minos( 60, 75 );
        Parameter e4 = Efficiency::Minos( 50, 60 );
        cout << "Direct value: " << FloatingPoint( Poisson::Minos( 50 ), 4 ) << endl;
        cout << "(a*b*c*d*e)" << FloatingPoint( Prod( a, e1, e2, e3, e4 ), 4 ) << endl;
        cout << "(a*b)*(c*d*e)" << FloatingPoint( Prod( Prod( a, e1 ), Prod( e2, e3, e4 ) ), 4 ) << endl;
        cout << "(a*b)*(c*d)*e" << FloatingPoint( Prod( Prod( a, e1 ), Prod( e2, e3 ), e4 ), 4 ) << endl;
        cout << "((((a*b)*c)*d)*e)" << FloatingPoint( Prod( Prod( Prod( Prod( a, e1 ), e2 ), e3 ), e4 ), 4 ) << endl;
    }
    cout << "\n>>> Another product test: Scale factor tests" << endl;
    {
        Parameter s( 1, 0, 0 );
        Parameter a( 0.9867, 0.0060, 0.0060 );
        Parameter b( 0.9791, 0.0132, 0.0132 );
        Parameter c( 0.9384, 0.0022, 0.0023 );
        cout << FloatingPoint( a, 3 ) << endl;
        cout << FloatingPoint( b, 3 ) << endl;
        cout << FloatingPoint( c, 3 ) << endl;
        cout << FloatingPoint( Prod( a, b, c ), 3 ) << endl;
        cout << FloatingPoint( s *= a, 3 ) << endl;
        cout << FloatingPoint( s *= b, 3 ) << endl;
        cout << FloatingPoint( s *= c, 3 ) << endl;
    }
    cout << "\n>> Specical case testing " << endl;
    {
        Parameter a( 11394.14521, +3455.26185, +2904.08486 );
        Parameter b( 1.65585, +1.88848, +0.83634 );
        cout << FloatingPoint( a, 5 ) << endl;
        cout << FloatingPoint( b, 5 ) << endl;
        a += b;
        cout << FloatingPoint( a, 5 ) << endl;
    }
    cout << "\n>>> Scale case testing" << endl;
    {
        Parameter a( 0, 0, 0 );
        Parameter b = Poisson::Minos( 10 );
        cout << FloatingPoint( b, 5 ) << endl;
        cout << FloatingPoint( a + b, 5 ) << endl;
        cout << FloatingPoint( b + a, 5 ) << endl;
    }
    cout << "\n>>> Scale product testing" << endl;
    {
        Parameter a( 1, 0.001, 0.2 );
        Parameter b( 100, 0.1, 0.2 );
        Parameter c( 1000, 100, 200 );
        cout << FloatingPoint( Prod( a, b ), 3 ) << endl;
        cout << FloatingPoint( Prod( b, c ), 3 ) << endl;
        // cout << FloatingPoint( Prod(c,b), 3 ) << endl;
        // cout << FloatingPoint( Prod(a,b,c), 3 ) << endl;
    }
    cout << "\n>>> Product test " << endl;
    {
        Parameter results = Prod(
            Parameter( 1, 0.0107266, 0.0107266 ),
            Parameter( 1, 0.0329748, 0.0279871 ),
            Parameter( 1,         0,  0.170623 ),
            Parameter( 1,  0.133084,   0.13232 ),
            Parameter( 1, 0.0243757, 0.0240791 ),
            Parameter( 1, 0.0257803, 0.0254051 ),
            Parameter( 1, 0.0249153, 0.0249153 ),
            Parameter( 1,  0.163553,  0.163553 ),
            Parameter( 1, 0.0547323, 0.0542217 ),
            Parameter( 1,     0.046,     0.046 ),
            Parameter( 1,      0.03,      0.03 )
            );
        cout << FloatingPoint( results, 3 ) << endl;
    }
    cout << "\n>>> Product test" << endl;
    {
        Parameter a( 1, 1.46, 0.787 );
        Parameter b( 1, 0.046, 0.046 );
        Parameter c( 1, 0.03, 0.03 );
        cout << FloatingPoint( Prod( a, b, c ), 5 ) << endl;
        cout << FloatingPoint( Prod( a, b ), 5 ) << endl;
        cout << FloatingPoint( Prod( b, c ), 5 ) << endl;
        cout << FloatingPoint( Prod( a, c ), 5 ) << endl;
    }
    return 0;
}


#include <Eigen/Dense>

#include "DenseMatrix.h"

struct RawSubmatrixData::Impl {
    Eigen::MatrixXd RawData;

  public:
    Impl() = default;
};

RawSubmatrixData::RawSubmatrixData() : pImpl {std::make_unique<Impl>()} {}

RawSubmatrixData::~RawSubmatrixData() = default;
RawSubmatrixData::RawSubmatrixData(RawSubmatrixData&&) noexcept = default;
RawSubmatrixData& RawSubmatrixData::operator=(RawSubmatrixData&&) noexcept = default;

void RawSubmatrixData::add_to_position(double value, uint32_t i, uint32_t j) {
    pImpl->RawData(i, j) += value;
}

std::ostream& operator<<(std::ostream& os, const RawSubmatrixData& decomposition) {
    os << decomposition.pImpl->RawData << std::endl;
    return os;
}

void RawSubmatrixData::resize(
    uint32_t matrix_in_space_basis_size_i,
    uint32_t matrix_in_space_basis_size_j) {
    // TODO: is it the fastest way to initialize pImpl->RawData?
    pImpl->RawData.resize(matrix_in_space_basis_size_i, matrix_in_space_basis_size_j);
    pImpl->RawData.fill(0);
}

void RawSubmatrixData::diagonalize() {
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es;
    es.compute(pImpl->RawData, Eigen::ComputeEigenvectors);
    std::cout << es.eigenvalues() << std::endl;
    std::cout << es.eigenvectors() << std::endl;
}


#include <iostream>
#include <cmath>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/extract_clusters.h>
#include <Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/common/angles.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

#include <ros/ros.h>
#include <boost/foreach.hpp>

#include <vision_module/obj_mass.h>
#include <vision_module/rotation_matrix.h>
#include <vision_module/bbox_size.h>

using namespace std;
typedef pcl::PointXYZ PointType;

static ros::Subscriber sub;
static ros::Publisher pub1;
static ros::Publisher pub2;
static ros::Publisher pub3;

// Get N bits of the string from back to front.
char* Substrend(char*str,int n)
{
	char *substr=(char*)malloc(n+1);
	int length=strlen(str);
	if (n>=length)
	{
		strcpy(substr,str);
		return substr;
	}
	int k=0;
	for (int i=length-n;i<length;i++)
	{
		substr[k]=str[i];
		k++;
	}
	substr[k]='\0';
	return substr;
}

void callback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud)
{
    printf ("Cloud: width = %d, height = %d\n", cloud->width, cloud->height);
    // TODO： process the data point.

    Eigen::Vector4f pcaCentroid;
    pcl::compute3DCentroid(*cloud, pcaCentroid);
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
    Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();

    //Correct vertical between main directions
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

    std::cout << "eigenValue(3x1):\n" << eigenValuesPCA << std::endl;
    std::cout << "eigenVector(3x3):\n" << eigenVectorsPCA << std::endl;
    std::cout << "mass center in camera coordinate system(4x1):\n" << pcaCentroid << std::endl;

    Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
    tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R.
    tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());//  -R*t
    tm_inv = tm.inverse();

    //std::cout << "transformation matrix(4x4):\n" << tm << std::endl;
    std::cout << "transformation matrix tm'(4x4):\n" << tm_inv << std::endl;

    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
    pcl::transformPointCloud(*cloud, *transformedCloud, tm);

    PointType min_p1, max_p1;
    Eigen::Vector3f c1, c;
    pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
    c1 = 0.5f*(min_p1.getVector3fMap() + max_p1.getVector3fMap());

    //std::cout << "center c1(3x1):\n" << c1 << std::endl;

    Eigen::Affine3f tm_inv_aff(tm_inv);
    pcl::transformPoint(c1, c, tm_inv_aff);

    Eigen::Vector3f whd, whd1;
    whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();
    whd = whd1;
    float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3;

    std::cout << "3D properties:"<< std::endl;
    std::cout << "mass center(3x1):\n" << pcaCentroid << std::endl;
    std::cout << "rotation matrix(3x3):\n" << tm_inv.block<3, 3>(0, 0) << std::endl;
    std::cout << "bounding box size:"<< std::endl;
    std::cout << "width=" << whd1(0) << endl;
    std::cout << "height=" << whd1(1) << endl;
    std::cout << "depth=" << whd1(2) << endl;
    //std::cout << "scale1=" << sc1 << endl;

    /*const Eigen::Quaternionf bboxQ(tm_inv.block<3, 3>(0, 0));
    const Eigen::Vector3f    bboxT(c);
    auto euler = bboxQ1.toRotationMatrix().eulerAngles(0, 1, 2);
    std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler/3.14*180 << std::endl;*/

    //Convert the camera coordinate position from the camera coordinate system to the world coordinate system.(1,1,1,0,90,-90)
    /*Eigen::Matrix4f tw = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f tw_inv = Eigen::Matrix4f::Identity();
    Eigen::Vector4f tw_centroid;
    tw(0,0) = 0.0; tw(0,1) = 1.0; tw(0,2) = 0.0; tw(0,3) = 1.0;
    tw(1,0) = 1.0; tw(1,1) = 0.0; tw(1,2) = 0.0; tw(1,3) = 1.0;
    tw(2,0) = 0.0; tw(2,1) = 0.0; tw(2,2) = -1.0; tw(2,3) = 1.0;
    tw_inv = tw.inverse();
    tw_centroid = tw_inv * pcaCentroid;
    std::cout << "mass center in World coordinate system(4x1):\n" << tw_centroid << std::endl;
    */

    vision_module::obj_mass o_mass;
    vision_module::rotation_matrix r_matrix;
    vision_module::bbox_size b_size;

    //publish mass center of object
    o_mass.x = tm_inv(0,3);
    o_mass.y = tm_inv(1,3);
    o_mass.z = tm_inv(2,3);
    pub1.publish(o_mass);

    //publish rotation matrix from object to camera
    r_matrix.r11 = tm_inv(0,0);
    r_matrix.r21 = tm_inv(1,0);
    r_matrix.r31 = tm_inv(2,0);
    r_matrix.r12 = tm_inv(0,1);
    r_matrix.r22 = tm_inv(1,1);
    r_matrix.r32 = tm_inv(2,1);
    r_matrix.r13 = tm_inv(0,2);
    r_matrix.r23 = tm_inv(1,2);
    r_matrix.r33 = tm_inv(2,2);
    pub2.publish(r_matrix);

    //publish mass center of object
    b_size.width = whd1(0);
    b_size.height = whd1(1);
    b_size.depth = whd1(2);
    pub3.publish(b_size);

    //pub.publish(pose_information);

}

int main(int argc, char** argv)
{
  std::cout<<"Init the pose information node:" <<std::endl;
  ros::init(argc, argv, "pose_information");
  ros::NodeHandle nh;
  sub = nh.subscribe<pcl::PointCloud<pcl::PointXYZ>> ("region_growing", 1000, callback);
  pub1 = nh.advertise<vision_module::obj_mass> ("obj_mass", 1000);
  pub2 = nh.advertise<vision_module::rotation_matrix> ("rotation_matrix", 1000);
  pub3 = nh.advertise<vision_module::bbox_size> ("BBox_size", 1000);
  ros::spin();
}


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    const unsigned n= 10;
    dense2D<float, mat::parameters<col_major> > B(n, n);

    mat::hessian_setup(B, 1.0);

    std::cout << "one_norm(B) is " << one_norm(B)<< "\n";
    std::cout << "infinity_norm(B) is " << infinity_norm(B)<< "\n";
    std::cout << "frobenius_norm(B) is " << frobenius_norm(B)<< "\n";

    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::DefaultDevice;

template <int DataLayout>
static void test_evals()
{
  Tensor<float, 2, DataLayout> input(3, 3);
  Tensor<float, 1, DataLayout> kernel(2);

  input.setRandom();
  kernel.setRandom();

  Tensor<float, 2, DataLayout> result(2,3);
  result.setZero();
  Eigen::array<Tensor<float, 2>::Index, 1> dims3{{0}};

  typedef TensorEvaluator<decltype(input.convolve(kernel, dims3)), DefaultDevice> Evaluator;
  Evaluator eval(input.convolve(kernel, dims3), DefaultDevice());
  eval.evalTo(result.data());
  EIGEN_STATIC_ASSERT(Evaluator::NumDims==2ul, YOU_MADE_A_PROGRAMMING_MISTAKE);
  VERIFY_IS_EQUAL(eval.dimensions()[0], 2);
  VERIFY_IS_EQUAL(eval.dimensions()[1], 3);

  VERIFY_IS_APPROX(result(0,0), input(0,0)*kernel(0) + input(1,0)*kernel(1));  // index 0
  VERIFY_IS_APPROX(result(0,1), input(0,1)*kernel(0) + input(1,1)*kernel(1));  // index 2
  VERIFY_IS_APPROX(result(0,2), input(0,2)*kernel(0) + input(1,2)*kernel(1));  // index 4
  VERIFY_IS_APPROX(result(1,0), input(1,0)*kernel(0) + input(2,0)*kernel(1));  // index 1
  VERIFY_IS_APPROX(result(1,1), input(1,1)*kernel(0) + input(2,1)*kernel(1));  // index 3
  VERIFY_IS_APPROX(result(1,2), input(1,2)*kernel(0) + input(2,2)*kernel(1));  // index 5
}

template <int DataLayout>
static void test_expr()
{
  Tensor<float, 2, DataLayout> input(3, 3);
  Tensor<float, 2, DataLayout> kernel(2, 2);
  input.setRandom();
  kernel.setRandom();

  Tensor<float, 2, DataLayout> result(2,2);
  Eigen::array<ptrdiff_t, 2> dims;
  dims[0] = 0;
  dims[1] = 1;
  result = input.convolve(kernel, dims);

  VERIFY_IS_APPROX(result(0,0), input(0,0)*kernel(0,0) + input(0,1)*kernel(0,1) +
                                input(1,0)*kernel(1,0) + input(1,1)*kernel(1,1));
  VERIFY_IS_APPROX(result(0,1), input(0,1)*kernel(0,0) + input(0,2)*kernel(0,1) +
                                input(1,1)*kernel(1,0) + input(1,2)*kernel(1,1));
  VERIFY_IS_APPROX(result(1,0), input(1,0)*kernel(0,0) + input(1,1)*kernel(0,1) +
                                input(2,0)*kernel(1,0) + input(2,1)*kernel(1,1));
  VERIFY_IS_APPROX(result(1,1), input(1,1)*kernel(0,0) + input(1,2)*kernel(0,1) +
                                input(2,1)*kernel(1,0) + input(2,2)*kernel(1,1));
}

template <int DataLayout>
static void test_modes() {
  Tensor<float, 1, DataLayout> input(3);
  Tensor<float, 1, DataLayout> kernel(3);
  input(0) = 1.0f;
  input(1) = 2.0f;
  input(2) = 3.0f;
  kernel(0) = 0.5f;
  kernel(1) = 1.0f;
  kernel(2) = 0.0f;

  Eigen::array<ptrdiff_t, 1> dims;
  dims[0] = 0;
  Eigen::array<std::pair<ptrdiff_t, ptrdiff_t>, 1> padding;

  // Emulate VALID mode (as defined in
  // http://docs.scipy.org/doc/numpy/reference/generated/numpy.convolve.html).
  padding[0] = std::make_pair(0, 0);
  Tensor<float, 1, DataLayout> valid(1);
  valid = input.pad(padding).convolve(kernel, dims);
  VERIFY_IS_EQUAL(valid.dimension(0), 1);
  VERIFY_IS_APPROX(valid(0), 2.5f);

  // Emulate SAME mode (as defined in
  // http://docs.scipy.org/doc/numpy/reference/generated/numpy.convolve.html).
  padding[0] = std::make_pair(1, 1);
  Tensor<float, 1, DataLayout> same(3);
  same = input.pad(padding).convolve(kernel, dims);
  VERIFY_IS_EQUAL(same.dimension(0), 3);
  VERIFY_IS_APPROX(same(0), 1.0f);
  VERIFY_IS_APPROX(same(1), 2.5f);
  VERIFY_IS_APPROX(same(2), 4.0f);

  // Emulate FULL mode (as defined in
  // http://docs.scipy.org/doc/numpy/reference/generated/numpy.convolve.html).
  padding[0] = std::make_pair(2, 2);
  Tensor<float, 1, DataLayout> full(5);
  full = input.pad(padding).convolve(kernel, dims);
  VERIFY_IS_EQUAL(full.dimension(0), 5);
  VERIFY_IS_APPROX(full(0), 0.0f);
  VERIFY_IS_APPROX(full(1), 1.0f);
  VERIFY_IS_APPROX(full(2), 2.5f);
  VERIFY_IS_APPROX(full(3), 4.0f);
  VERIFY_IS_APPROX(full(4), 1.5f);
}

template <int DataLayout>
static void test_strides() {
  Tensor<float, 1, DataLayout> input(13);
  Tensor<float, 1, DataLayout> kernel(3);
  input.setRandom();
  kernel.setRandom();

  Eigen::array<ptrdiff_t, 1> dims;
  dims[0] = 0;
  Eigen::array<ptrdiff_t, 1> stride_of_3;
  stride_of_3[0] = 3;
  Eigen::array<ptrdiff_t, 1> stride_of_2;
  stride_of_2[0] = 2;

  Tensor<float, 1, DataLayout> result;
  result = input.stride(stride_of_3).convolve(kernel, dims).stride(stride_of_2);

  VERIFY_IS_EQUAL(result.dimension(0), 2);
  VERIFY_IS_APPROX(result(0), (input(0)*kernel(0) + input(3)*kernel(1) +
                               input(6)*kernel(2)));
  VERIFY_IS_APPROX(result(1), (input(6)*kernel(0) + input(9)*kernel(1) +
                               input(12)*kernel(2)));
}

void test_cxx11_tensor_convolution()
{
  CALL_SUBTEST(test_evals<ColMajor>());
  CALL_SUBTEST(test_evals<RowMajor>());
  CALL_SUBTEST(test_expr<ColMajor>());
  CALL_SUBTEST(test_expr<RowMajor>());
  CALL_SUBTEST(test_modes<ColMajor>());
  CALL_SUBTEST(test_modes<RowMajor>());
  CALL_SUBTEST(test_strides<ColMajor>());
  CALL_SUBTEST(test_strides<RowMajor>());
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/detail/adams_bashforth_coefficients.hpp

 [begin_description]
 Definition of the coefficients for the Adams-Bashforth method.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

template< class Value , size_t Steps >
class adams_bashforth_coefficients ;

template< class Value >
class adams_bashforth_coefficients< Value , 1 > : public boost::array< Value , 1 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 1 >()
      {
        (*this)[0] = static_cast< Value >( 1 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 2 > : public boost::array< Value , 2 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 2 >()
      {
        (*this)[0] = static_cast< Value >( 3 ) / static_cast< Value >( 2 );
        (*this)[1] = -static_cast< Value >( 1 ) / static_cast< Value >( 2 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 3 > : public boost::array< Value , 3 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 3 >()
      {
        (*this)[0] = static_cast< Value >( 23 ) / static_cast< Value >( 12 );
        (*this)[1] = -static_cast< Value >( 4 ) / static_cast< Value >( 3 );
        (*this)[2] = static_cast< Value >( 5 ) / static_cast< Value >( 12 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 4 > : public boost::array< Value , 4 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 4 >()
      {
        (*this)[0] = static_cast< Value >( 55 ) / static_cast< Value >( 24 );
        (*this)[1] = -static_cast< Value >( 59 ) / static_cast< Value >( 24 );
        (*this)[2] = static_cast< Value >( 37 ) / static_cast< Value >( 24 );
        (*this)[3] = -static_cast< Value >( 3 ) / static_cast< Value >( 8 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 5 > : public boost::array< Value , 5 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 5 >()
      {
        (*this)[0] = static_cast< Value >( 1901 ) / static_cast< Value >( 720 );
        (*this)[1] = -static_cast< Value >( 1387 ) / static_cast< Value >( 360 );
        (*this)[2] = static_cast< Value >( 109 ) / static_cast< Value >( 30 );
        (*this)[3] = -static_cast< Value >( 637 ) / static_cast< Value >( 360 );
        (*this)[4] = static_cast< Value >( 251 ) / static_cast< Value >( 720 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 6 > : public boost::array< Value , 6 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 6 >()
      {
        (*this)[0] = static_cast< Value >( 4277 ) / static_cast< Value >( 1440 );
        (*this)[1] = -static_cast< Value >( 2641 ) / static_cast< Value >( 480 );
        (*this)[2] = static_cast< Value >( 4991 ) / static_cast< Value >( 720 );
        (*this)[3] = -static_cast< Value >( 3649 ) / static_cast< Value >( 720 );
        (*this)[4] = static_cast< Value >( 959 ) / static_cast< Value >( 480 );
        (*this)[5] = -static_cast< Value >( 95 ) / static_cast< Value >( 288 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 7 > : public boost::array< Value , 7 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 7 >()
      {
        (*this)[0] = static_cast< Value >( 198721 ) / static_cast< Value >( 60480 );
        (*this)[1] = -static_cast< Value >( 18637 ) / static_cast< Value >( 2520 );
        (*this)[2] = static_cast< Value >( 235183 ) / static_cast< Value >( 20160 );
        (*this)[3] = -static_cast< Value >( 10754 ) / static_cast< Value >( 945 );
        (*this)[4] = static_cast< Value >( 135713 ) / static_cast< Value >( 20160 );
        (*this)[5] = -static_cast< Value >( 5603 ) / static_cast< Value >( 2520 );
        (*this)[6] = static_cast< Value >( 19087 ) / static_cast< Value >( 60480 );
      }
};

template< class Value >
class adams_bashforth_coefficients< Value , 8 > : public boost::array< Value , 8 >
{
public:
    adams_bashforth_coefficients( void )
    : boost::array< Value , 8 >()
      {
        (*this)[0] = static_cast< Value >( 16083 ) / static_cast< Value >( 4480 );
        (*this)[1] = -static_cast< Value >( 1152169 ) / static_cast< Value >( 120960 );
        (*this)[2] = static_cast< Value >( 242653 ) / static_cast< Value >( 13440 );
        (*this)[3] = -static_cast< Value >( 296053 ) / static_cast< Value >( 13440 );
        (*this)[4] = static_cast< Value >( 2102243 ) / static_cast< Value >( 120960 );
        (*this)[5] = -static_cast< Value >( 115747 ) / static_cast< Value >( 13440 );
        (*this)[6] = static_cast< Value >( 32863 ) / static_cast< Value >( 13440 );
        (*this)[7] = -static_cast< Value >( 5257 ) / static_cast< Value >( 17280 );
      }
};

} // detail
} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED


// Test ../include/Spectra/LinAlg/BKLDLT.h
#include <Eigen/Core>
#include <Spectra/LinAlg/BKLDLT.h>

using namespace Spectra;

#define CATCH_CONFIG_MAIN
#include "catch.hpp"

using Eigen::MatrixXd;
using Eigen::VectorXd;

// Solve (A - s * I)x = b
void run_test(const MatrixXd& A, const VectorXd& b, double s)
{
    BKLDLT<double> decompL(A, Eigen::Lower, s);
    REQUIRE(decompL.info() == SUCCESSFUL);

    BKLDLT<double> decompU(A, Eigen::Upper, s);
    REQUIRE(decompU.info() == SUCCESSFUL);

    VectorXd solL = decompL.solve(b);
    VectorXd solU = decompU.solve(b);
    REQUIRE((solL - solU).cwiseAbs().maxCoeff() == 0.0);

    const double tol = 1e-9;
    VectorXd resid = A * solL - s * solL - b;
    INFO("||(A - s * I)x - b||_inf = " << resid.cwiseAbs().maxCoeff());
    REQUIRE(resid.cwiseAbs().maxCoeff() == Approx(0.0).margin(tol));
}

TEST_CASE("BKLDLT decomposition of symmetric real matrix [10x10]", "[BKLDLT]")
{
    std::srand(123);
    const int n = 10;
    MatrixXd A = MatrixXd::Random(n, n);
    A = (A + A.transpose()).eval();
    VectorXd b = VectorXd::Random(n);
    const double shift = 1.0;

    run_test(A, b, shift);
}

TEST_CASE("BKLDLT decomposition of symmetric real matrix [100x100]", "[BKLDLT]")
{
    std::srand(123);
    const int n = 100;
    MatrixXd A = MatrixXd::Random(n, n);
    A = (A + A.transpose()).eval();
    VectorXd b = VectorXd::Random(n);
    const double shift = 1.0;

    run_test(A, b, shift);
}

TEST_CASE("BKLDLT decomposition of symmetric real matrix [1000x1000]", "[BKLDLT]")
{
    std::srand(123);
    const int n = 1000;
    MatrixXd A = MatrixXd::Random(n, n);
    A = (A + A.transpose()).eval();
    VectorXd b = VectorXd::Random(n);
    const double shift = 1.0;

    run_test(A, b, shift);
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019, The University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
//
// This file was originally part of Exotica, cf.
// https://github.com/ipab-slmc/exotica/blob/master/exotica_core/include/exotica_core/tools/box_qp.h
//
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_
#define CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_

#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <vector>

namespace crocoddyl {
struct BoxQPSolution {
  BoxQPSolution(const Eigen::MatrixXd& Hff_inv_in, const Eigen::VectorXd& x_in, const std::vector<size_t>& free_idx_in,
                const std::vector<size_t>& clamped_idx_in)
      : Hff_inv(Hff_inv_in), x(x_in), free_idx(free_idx_in), clamped_idx(clamped_idx_in) {}

  Eigen::MatrixXd Hff_inv;
  Eigen::MatrixXd x;
  std::vector<size_t> free_idx;
  std::vector<size_t> clamped_idx;
};

// Based on Yuval Tassa's BoxQP
// Cf. https://www.mathworks.com/matlabcentral/fileexchange/52069-ilqg-ddp-trajectory-optimization
inline BoxQPSolution BoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& b_low,
                           const Eigen::VectorXd& b_high, const Eigen::VectorXd& x_init, const double gamma,
                           const int max_iterations, const double epsilon, const double lambda) {
  if (max_iterations < 0) {
    throw std::runtime_error("Max iterations needs to be positive.");
  }

  int it = 0;
  Eigen::VectorXd delta_xf(x_init.size()), x = x_init;
  std::vector<size_t> clamped_idx, free_idx;
  Eigen::VectorXd grad = q + H * x_init;
  Eigen::MatrixXd Hff, Hfc, Hff_inv(H.rows(), H.cols());
  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt = Eigen::LLT<Eigen::MatrixXd>(H.rows());

  Hff_inv_llt.compute(Eigen::MatrixXd::Identity(H.rows(), H.cols()) * lambda + H);
  Hff_inv_llt.solveInPlace(Hff_inv);

  if (grad.lpNorm<Eigen::Infinity>() <= epsilon) {
    return BoxQPSolution(Hff_inv, x_init, free_idx, clamped_idx);
  }

  while (grad.lpNorm<Eigen::Infinity>() > epsilon && it < max_iterations) {
    ++it;
    grad.noalias() = q + H * x;
    clamped_idx.clear();
    free_idx.clear();

    for (int i = 0; i < grad.size(); ++i) {
      if ((x(i) == b_low(i) && grad(i) > 0) || (x(i) == b_high(i) && grad(i) < 0)) {
        clamped_idx.push_back(i);
      } else {
        free_idx.push_back(i);
      }
    }

    if (free_idx.size() == 0) {
      return BoxQPSolution(Hff_inv, x, free_idx, clamped_idx);
    }

    Hff.resize(free_idx.size(), free_idx.size());
    Hfc.resize(free_idx.size(), clamped_idx.size());

    if (clamped_idx.size() == 0) {
      Hff = H;
    } else {
      for (size_t i = 0; i < free_idx.size(); ++i) {
        for (size_t j = 0; j < free_idx.size(); ++j) {
          Hff(i, j) = H(free_idx[i], free_idx[j]);
        }
      }

      for (size_t i = 0; i < free_idx.size(); ++i) {
        for (size_t j = 0; j < clamped_idx.size(); ++j) {
          Hfc(i, j) = H(free_idx[i], clamped_idx[j]);
        }
      }
    }

    // NOTE: Array indexing not supported in current eigen version
    Eigen::VectorXd q_free(free_idx.size()), x_free(free_idx.size()), x_clamped(clamped_idx.size());
    for (size_t i = 0; i < free_idx.size(); ++i) {
      q_free(i) = q(free_idx[i]);
      x_free(i) = x(free_idx[i]);
    }

    for (size_t j = 0; j < clamped_idx.size(); ++j) {
      x_clamped(j) = x(clamped_idx[j]);
    }

    // The dimension of Hff has changed - reinitialise LLT
    // Hff_inv_llt = Eigen::LLT<Eigen::MatrixXd>(Hff.rows());
    // Hff_inv_llt.compute(Eigen::MatrixXd::Identity(Hff.rows(), Hff.cols()) * lambda + Hff);
    // Hff_inv_llt.solveInPlace(Hff_inv);
    // TODO: Use Cholesky, however, often unstable without adapting lambda.
    Hff_inv = (Eigen::MatrixXd::Identity(Hff.rows(), Hff.cols()) * lambda + Hff).inverse();

    if (clamped_idx.size() == 0) {
      // std::cout << "Hff_inv=" << Hff_inv.rows() << "x" << Hff_inv.cols() << ", q_free=" << q_free.size() << ",
      // x_free=" << x_free.size() << std::endl;
      delta_xf.noalias() = -Hff_inv * (q_free)-x_free;
    } else {
      delta_xf.noalias() = -Hff_inv * (q_free + Hfc * x_clamped) - x_free;
    }

    double f_old = (0.5 * x.transpose() * H * x + q.transpose() * x)(0);
    static const Eigen::VectorXd alpha_space = Eigen::VectorXd::LinSpaced(10, 1.0, 0.1);

    bool armijo_reached = false;
    Eigen::VectorXd x_new(x.size()), x_diff(x.size());
    for (int ai = 0; ai < alpha_space.rows(); ++ai) {
      x_new = x;
      for (size_t i = 0; i < free_idx.size(); ++i) {
        x_new(free_idx[i]) = std::max(std::min(x(free_idx[i]) + alpha_space[ai] * delta_xf(i), b_high(i)), b_low(i));
      }

      double f_new = (0.5 * x_new.transpose() * H * x_new + q.transpose() * x_new)(0);
      x_diff.noalias() = x - x_new;

      // armijo criterion>
      double armijo_coef = (f_old - f_new) / (grad.transpose() * x_diff + 1e-5);
      if (armijo_coef > gamma) {
        armijo_reached = true;
        x = x_new;
        break;
      }
    }

    // break if no step made
    if (!armijo_reached) break;
  }

  return BoxQPSolution(Hff_inv, x, free_idx, clamped_idx);
}

inline BoxQPSolution BoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& b_low,
                           const Eigen::VectorXd& b_high, const Eigen::VectorXd& x_init) {
  const double epsilon = 1e-5;
  const double gamma = 0.1;
  const int max_iterations = 100;
  const double lambda = 1e-5;
  return BoxQP(H, q, b_low, b_high, x_init, gamma, max_iterations, epsilon, lambda);
}
}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_BOX_QP_HPP_


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "rttbLinearInterpolation.h"

#include <boost/make_shared.hpp>

namespace rttb
{
	namespace interpolation
	{

		DoseTypeGy LinearInterpolation::trilinear(std::array<double, 3> target,
		        boost::shared_ptr<DoseTypeGy[]> values) const
		{
			//4 linear interpolation in x direction
			DoseTypeGy c_00 = values[0] * (1.0 - target[0]) + values[1] * target[0];
			DoseTypeGy c_10 = values[2] * (1.0 - target[0]) + values[3] * target[0];
			DoseTypeGy c_01 = values[4] * (1.0 - target[0]) + values[5] * target[0];
			DoseTypeGy c_11 = values[6] * (1.0 - target[0]) + values[7] * target[0];

			//combine result in y direction
			DoseTypeGy c_0 = c_00 * (1.0 - target[1]) + c_10 * target[1];
			DoseTypeGy c_1 = c_01 * (1.0 - target[1]) + c_11 * target[1];

			//finally incorporate z direction
			return (c_0 * (1.0 - target[2]) + c_1 * target[2]);
		}

		DoseTypeGy LinearInterpolation::getValue(const WorldCoordinate3D& aWorldCoordinate) const
		{
			//proper initialization of target and values
			std::array<double, 3> target = {0.0, 0.0, 0.0};
      auto values = boost::make_shared<DoseTypeGy[]>(8);
			getNeighborhoodVoxelValues(aWorldCoordinate, 8, target, values);

			return trilinear(target, values);
		}

	}
}


#include <mex.h>
#include <math.h>
#include <iostream>
#include <vector>

#include <igl/matlab/MexStream.h>
#include <igl/matlab/parse_rhs.h>
#include <igl/matlab/prepare_lhs.h>
#include <igl/matlab/validate_arg.h>

#include <igl/PI.h>
#include <igl/forward_kinematics.h>
#include <igl/directed_edge_parents.h>

#include <Eigen/Core>

using namespace Eigen;
using namespace std;

void euler_to_quat(const Eigen::Vector3d& euler, Eigen::Quaterniond& q) {

  q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
      * Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
}

void euler_to_quat(const Eigen::Vector3d& euler, const Eigen::Affine3d& a, Eigen::Quaterniond& q) {
  q = Eigen::AngleAxisd(euler(2), a.rotation().col(2))
      * Eigen::AngleAxisd(euler(1), a.rotation().col(1))
      * Eigen::AngleAxisd(euler(0), a.rotation().col(0));
  // q = Eigen::AngleAxisd(euler(0), a.rotation().col(0))
  //     * Eigen::AngleAxisd(euler(1), a.rotation().col(1))
  //     * Eigen::AngleAxisd(euler(2), a.rotation().col(2));
}

void read_pnt_anim(std::string anim_file,
    const Eigen::MatrixXd& C,
    const Eigen::VectorXd& center,
    const double& scale,
    std::vector<Eigen::MatrixXd>& T_list)
{
  FILE* file;
  file= fopen(anim_file.c_str(), "rb");

  int dim = 3;//= C.cols();

  double degree2radian = igl::PI/180.0;

  int num_bone, num_frame;
  double val = 0;

  typedef std::vector<Eigen::Quaterniond,
      Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

  fscanf(file, "%d %d\n", &num_bone, &num_frame);
  T_list.resize(num_frame);
  //std::cout<<"num_bone: "<<num_bone<<", num_frame: "<<num_frame<<std::endl;

  std::vector<Eigen::Vector3d> rest_tran_list(num_bone);
  std::vector<Eigen::Affine3d> rest_affine_list(num_bone);

  for(int i=0; i<num_bone ; i++){
    Eigen::Vector3d rest_tran;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      rest_tran(j) = val;
    }
    rest_tran_list[i] = (rest_tran-center)/scale;

    Eigen::Vector3d euler;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      euler(j) = val;
    }
    euler = euler.array() * degree2radian;
    Eigen::Quaterniond q;
    euler_to_quat(euler, q);
    Eigen::Affine3d root_affine = Eigen::Affine3d::Identity();
    root_affine.rotate(q);
    rest_affine_list[i] = root_affine;
    //std::cout<<"i: "<<i<<", mat: "<<root_affine.matrix()<<std::endl;
  }
  RotationList rot_list(num_bone);
  std::vector<Eigen::Vector3d> tran_list(num_bone);

  for (int k = 0; k < num_frame; k++) {
    for (int i = 0; i < num_bone; i++) {
      Eigen::Vector3d tran;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        tran(j) = val;
      }
      tran = (tran - center)/scale;
      Eigen::Vector3d euler;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        euler(j) = val;
      }
      euler = euler.array() * degree2radian;
      Eigen::Quaterniond q;
      euler_to_quat(euler, rest_affine_list[i], q);
      //std::cout<<"1 i: "<<i<<", mat: "<<rest_affine_list[i].rotation()<<std::endl;

      q = rest_affine_list[i].rotation().transpose()*q;

      //euler_to_quat(euler, q);

      //std::cout<<"q w: "<<q.w()<<"< vec: "<<q.vec()<<std::endl;

      const Eigen::Vector3d cn = C.row(i).transpose();

      tran_list[i] = tran - rest_tran_list[i] + cn-q*cn;
      //tran_list[i] = tran - rest_tran_list[i];
      rot_list[i] = q;
    }
    RotationList vQ = rot_list;
    std::vector<Eigen::Vector3d> vT= tran_list;
    //igl::forward_kinematics(C, BE, P, rot_list, tran_list, vQ, vT);

    Eigen::MatrixXd T(num_bone * (dim+1), dim);
    for (int i = 0; i < num_bone; i++) {
      Eigen::Affine3d a = Eigen::Affine3d::Identity();
      a.translate(vT[i]);
      a.rotate(vQ[i]);
      T.block(i * (dim+1), 0, dim+1, dim) = a.matrix().transpose().block(0, 0, dim+1, dim);
    }
    T_list[k] = T;
  }
  fclose(file);

}

// void read_pnt_anim(std::string anim_file,
//     std::vector<Eigen::MatrixXd>& T_list)
// {
//   FILE* file;
//   file= fopen(anim_file.c_str(), "rb");

//   int dim = 3;//= C.cols();

//   double degree2radian = igl::PI/180.0;

//   int num_bone, num_frame;
//   double val = 0;

//   typedef std::vector<Eigen::Quaterniond,
//       Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

//   fscanf(file, "%d %d\n", &num_bone, &num_frame);
//   T_list.resize(num_frame);
//   //std::cout<<"num_bone: "<<num_bone<<", num_frame: "<<num_frame<<std::endl;

//   std::vector<Eigen::Vector3d> rest_tran_list(num_bone);
//   std::vector<Eigen::Affine3d> rest_affine_list(num_bone);

//   for(int i=0; i<num_bone ; i++){
//     Eigen::Vector3d rest_tran;
//     for (int j = 0; j < dim; j++) {
//       fscanf(file, "%lf", &val);
//       rest_tran(j) = val;
//     }
//     rest_tran_list[i] = rest_tran;

//     Eigen::Vector3d euler;
//     for (int j = 0; j < dim; j++) {
//       fscanf(file, "%lf", &val);
//       euler(j) = val;
//     }
//     euler = euler.array() * degree2radian;
//     Eigen::Quaterniond q;
//     euler_to_quat(euler, q);
//     Eigen::Affine3d root_affine = Eigen::Affine3d::Identity();
//     root_affine.rotate(q);
//     rest_affine_list[i] = root_affine;
//   }
//   RotationList rot_list(num_bone);
//   std::vector<Eigen::Vector3d> tran_list(num_bone);

//   for (int k = 0; k < num_frame; k++) {
//     for (int i = 0; i < num_bone; i++) {
//       Eigen::Vector3d tran;
//       for (int j = 0; j < dim; j++) {
//         fscanf(file, "%lf", &val);
//         tran(j) = val;
//       }
//       Eigen::Vector3d euler;
//       for (int j = 0; j < dim; j++) {
//         fscanf(file, "%lf", &val);
//         euler(j) = val;
//       }
//       euler = euler.array() * degree2radian;
//       Eigen::Quaterniond q;
//       euler_to_quat(euler, rest_affine_list[i], q);

//       tran_list[i] = tran - rest_tran_list[i];
//       rot_list[i] = q;
//     }
//     RotationList vQ = rot_list;
//     std::vector<Eigen::Vector3d> vT= tran_list;
//     //igl::forward_kinematics(C, BE, P, rot_list, tran_list, vQ, vT);

//     Eigen::MatrixXd T(num_bone * (dim+1), dim);
//     for (int i = 0; i < num_bone; i++) {
//       Eigen::Affine3d a = Eigen::Affine3d::Identity();
//       a.translate(vT[i]);
//       a.rotate(vQ[i]);
//       T.block(i * (dim+1), 0, dim+1, dim) = a.matrix().transpose().block(0, 0, dim+1, dim);
//     }
//     T_list[k] = T;
//   }
//   fclose(file);
// }

void read_pnt_anim(std::string anim_file,
    Eigen::MatrixXd C,
    std::vector<Eigen::MatrixXd>& T_list)
{
  FILE* file;
  file= fopen(anim_file.c_str(), "rb");

  int dim = 3;//= C.cols();

  double degree2radian = igl::PI/180.0;

  int num_bone, num_frame;
  double val = 0;

  typedef std::vector<Eigen::Quaterniond,
      Eigen::aligned_allocator<Eigen::Quaterniond> > RotationList;

  fscanf(file, "%d %d\n", &num_bone, &num_frame);
  T_list.resize(num_frame);
  //std::cout<<"num_bone: "<<num_bone<<", num_frame: "<<num_frame<<std::endl;

  std::vector<Eigen::Vector3d> rest_tran_list(num_bone);
  std::vector<Eigen::Affine3d> rest_affine_list(num_bone);

  for(int i=0; i<num_bone ; i++){
    Eigen::Vector3d rest_tran;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      rest_tran(j) = val;
    }
    rest_tran_list[i] = rest_tran;

    Eigen::Vector3d euler;
    for (int j = 0; j < dim; j++) {
      fscanf(file, "%lf", &val);
      euler(j) = val;
    }
    euler = euler.array() * degree2radian;
    Eigen::Quaterniond q;
    euler_to_quat(euler, q);
    Eigen::Affine3d root_affine = Eigen::Affine3d::Identity();
    root_affine.rotate(q);
    rest_affine_list[i] = root_affine;
  }
  RotationList rot_list(num_bone);
  std::vector<Eigen::Vector3d> tran_list(num_bone);

  for (int k = 0; k < num_frame; k++) {
    for (int i = 0; i < num_bone; i++) {
      Eigen::Vector3d tran;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        tran(j) = val;
      }
      Eigen::Vector3d euler;
      for (int j = 0; j < dim; j++) {
        fscanf(file, "%lf", &val);
        euler(j) = val;
      }
      euler = euler.array() * degree2radian;
      Eigen::Quaterniond q;
      euler_to_quat(euler, rest_affine_list[i], q);

      q = rest_affine_list[i].rotation().transpose()*q;

      const Eigen::Vector3d cn = C.row(i).transpose();

      tran_list[i] = tran - rest_tran_list[i] + cn-q*cn;
      rot_list[i] = q;//cn-q*cn;//q;
    }
    RotationList vQ = rot_list;
    std::vector<Eigen::Vector3d> vT= tran_list;
    //igl::forward_kinematics(C, BE, P, rot_list, tran_list, vQ, vT);

    Eigen::MatrixXd T(num_bone * (dim+1), dim);
    for (int i = 0; i < num_bone; i++) {
      Eigen::Affine3d a = Eigen::Affine3d::Identity();
      a.translate(vT[i]);
      a.rotate(vQ[i]);
      T.block(i * (dim+1), 0, dim+1, dim) = a.matrix().transpose().block(0, 0, dim+1, dim);
    }
    T_list[k] = T;
  }
  fclose(file);
}

void mexFunction(
  int          nlhs,
  mxArray      *plhs[],
  int          nrhs,
  const mxArray *prhs[])
{
    Eigen::MatrixXd C;
    Eigen::MatrixXi BE;
    Eigen::VectorXi P;
    Eigen::VectorXd center;
    double scale;
    char* anim_file = mxArrayToString(prhs[0]);

    igl::matlab::parse_rhs_double(prhs+1, C);
    igl::matlab::parse_rhs_double(prhs+2, center);
    scale = (double) *mxGetPr(prhs[3]);

    std::vector<Eigen::MatrixXd> T_list;
    //read_pnt_anim(anim_file, center, scale, T_list);
    //read_pnt_anim(anim_file, C, T_list);
    read_pnt_anim(anim_file, C, center, scale, T_list);

    plhs[0] = mxCreateCellMatrix(T_list.size(),1);

    mxArray *x;
    for(int i=0; i<T_list.size(); i++){
        const int m = T_list[i].rows();
        const int n = T_list[i].cols();
        x = mxCreateDoubleMatrix(m,n, mxREAL);
        Eigen::Map< Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> > map(mxGetPr(x),m,n);
        map = T_list[i].template cast<double>();
        mxSetCell(plhs[0], i, x);
    }
    return;
}