#ifndef SKYLARK_ML_MODEL_HPP
#define SKYLARK_ML_MODEL_HPP

#include <El.hpp>
#include <El/core/types.h>
#include <skylark.hpp>
#include <cmath>
#include <boost/mpi.hpp>
#include <sstream>
#include <fstream>
#include <iostream>
#include <cstdlib>
#include <string>
#include <vector>
#include "kernels.hpp"
#include "options.hpp"

#ifdef SKYLARK_HAVE_OPENMP
#include <omp.h>
#endif

namespace skylark { namespace ml {

int classification_accuracy(El::Matrix<double>& Yt, El::Matrix<double>& Yp) {
    int correct = 0;
    double o, o1;
    int pred;

    for(int i=0; i < Yp.Height(); i++) {
        o = Yp.Get(i,0);
        pred = 0;
        if (Yp.Width()==1)
            pred = (o >= 0)? +1:-1;

        for(int j=1; j < Yp.Width(); j++) {
            o1 = Yp.Get(i,j);
            if ( o1 > o) {
                o = o1;
                pred = j;
            }
        }

        if(pred == (int) Yt.Get(i,0))
            correct++;
    }
    return correct;
}

struct hilbert_model_t {
    // TODO the following two should depend on the input type
    // TODO explicit doubles is not desired.
    typedef El::Matrix<double> intermediate_type;
    typedef El::Matrix<double> coef_type;

    typedef sketch::sketch_transform_t<boost::any, boost::any>
    feature_transform_type;

    template<typename SketchTransformType>
    hilbert_model_t(std::vector<const SketchTransformType *>& maps, bool scale_maps,
        int num_features, int num_outputs, bool regression) :
        _coef(num_features, num_outputs), _maps(maps.size()), _scale_maps(scale_maps),
        _regression(regression), _starts(maps.size()), _finishes(maps.size()) {

        // TODO verify all N dimension of the maps match

        El::Zero(_coef);

        int nf = 0;
        for(int i = 0; i < maps.size(); i++) {
            _maps[i] = maps[i]->type_erased();
            _starts[i] = nf;
            _finishes[i] = nf + _maps[i]->get_S() - 1;
            nf += _maps[i]->get_S();
        }

        _input_size = (_maps.size() == 0) ?
            num_features : _maps[0]->get_N();
    }

    hilbert_model_t(const boost::property_tree::ptree &pt) {
        build_from_ptree(pt);
    }

    hilbert_model_t(const std::string& fname) {
        std::ifstream is(fname);

        // Skip all lines begining with "#"
        while(is.peek() == '#')
            is.ignore(std::numeric_limits<std::streamsize>::max(), '\n');

        boost::property_tree::ptree pt;
        boost::property_tree::read_json(is, pt);
        is.close();
        build_from_ptree(pt);
    }

    ~hilbert_model_t() {
        for (auto it = _maps.begin(); it != _maps.end(); it++)
            delete *it;
    }

    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;
        pt.put("skylark_object_type", "model:linear-on-features");
        pt.put("skylark_version", VERSION);

        pt.put("num_features", _coef.Height());
        pt.put("num_outputs", _coef.Width());
        pt.put("input_size", _input_size);
        pt.put("regression", _regression);

        boost::property_tree::ptree ptfmap;
        ptfmap.put("number_maps", _maps.size());
        ptfmap.put("scale_maps", _scale_maps);

        boost::property_tree::ptree ptmaps;
        for(int i = 0; i < _maps.size(); i++)
            ptmaps.push_back(std::make_pair(std::to_string(i),
                    _maps[i]->to_ptree()));
        ptfmap.add_child("maps", ptmaps);

        pt.add_child("feature_mapping", ptfmap);

        std::stringstream scoef;
        El::Print(_coef, "", scoef);
        pt.put("coef_matrix", scoef.str());

        return pt;
    }

    /**
     * Saves the model to a file named fname. You may want to use this method
     * from only a single rank.
     */
    void save(const std::string& fname, const std::string& header) const {
        boost::property_tree::ptree pt = to_ptree();
        std::ofstream of(fname);
        of << header;
        boost::property_tree::write_json(of, pt);
        of.close();
    }

    template<typename InputType, typename LabelType, typename DecisionType>
    void predict(const InputType& X, LabelType& PV, DecisionType& DV,
        int num_threads = 1) const {

        int d = base::Height(X);
        int k = base::Width(_coef);
        int n = base::Width(X);

        if (_maps.size() == 0)  {
            // No maps (linear case)

            DV.Resize(n, k);
            base::Gemm(El::TRANSPOSE,El::NORMAL,1.0, X, _coef, 0.0, DV);
        } else {
            // Non-linear case
            coef_type Wslice;
            int j, start, finish, sj;

            El::Zeros(DV, n, k);
#           ifdef SKYLARK_HAVE_OPENMP
#           pragma omp parallel for if(num_threads > 1) private(j, start, finish, sj) num_threads(num_threads)
#           endif
            for(j = 0; j < _maps.size(); j++) {
                start = _starts[j];
                finish = _finishes[j];
                sj = finish - start  + 1;

                intermediate_type z(sj, n);
                _maps[j]->apply(&X, &z, sketch::columnwise_tag());

                if (_scale_maps)
                    // TODO shouldn't it be s instead of d?
                    El::Scale(sqrt(double(sj) / d), z);

                DecisionType o(n, k);

                El::LockedView(Wslice, _coef, start, 0, sj, k);
                base::Gemm(El::TRANSPOSE, El::NORMAL, 1.0, z, Wslice, o);

#               ifdef SKYLARK_HAVE_OPENMP
#               pragma omp critical
#               endif
                base::Axpy(+1.0, o, DV);
            }
        }

        if (!_regression) {
            double o, o1, pred;
            PV.Resize(n, 1);
            for(int i=0; i < DV.Height(); i++) {
                o = DV.Get(i,0);
                pred = 0;
                if (DV.Width()==1)
                    pred = (o >= 0)? +1:-1;

                for(int j=1; j < DV.Width(); j++) {
                    o1 = DV.Get(i,j);
                    if ( o1 > o) {
                        o = o1;
                        pred = j;
                    }
                }
                PV.Set(i,0, pred);
            }
        }
    }

    coef_type& get_coef() { return _coef; }

    int get_output_size() const { return _coef.Width(); }
    int get_input_size() const { return _input_size; }

    bool is_regression() const { return _regression; }

protected:

    void build_from_ptree(const boost::property_tree::ptree &pt) {
        int num_features = pt.get<int>("num_features");
        int num_outputs = pt.get<int>("num_outputs");
        _coef.Resize(num_features, num_outputs);

        _input_size = pt.get<int>("input_size");
        _regression = pt.get<bool>("regression");

        int num_maps = pt.get<int>("feature_mapping.number_maps");
        _maps.resize(num_maps);
        const boost::property_tree::ptree &ptmaps =
            pt.get_child("feature_mapping.maps");
        for(int i = 0; i < num_maps; i++)
            _maps[i] =
                feature_transform_type::from_ptree(
                   ptmaps.get_child(std::to_string(i)));

        int nf = 0;
        _starts.resize(num_maps);
        _finishes.resize(num_maps);
        for(int i = 0; i < _maps.size(); i++) {
            _starts[i] = nf;
            _finishes[i] = nf + _maps[i]->get_S() - 1;
            nf += _maps[i]->get_S();
        }

        _scale_maps = pt.get<bool>("feature_mapping.scale_maps");

        std::istringstream coef_str(pt.get<std::string>("coef_matrix"));
        double *buffer = _coef.Buffer();
        int ldim = _coef.LDim();
        for(int i = 0; i < num_features; i++) {
            std::string line;
            std::getline(coef_str, line);
            std::istringstream coefstream(line);
            for(int j = 0; j < num_outputs; j++) {
                std::string token;
                coefstream >> token;
                buffer[i + j * ldim] = atof(token.c_str());
            }
        }
    }

private:
    coef_type _coef;
    El::Int _input_size;
    std::vector<const feature_transform_type *> _maps; // TODO use shared_ptr
    bool _scale_maps;
    bool _regression;

    std::vector<int> _starts, _finishes;
};

//////////////////////////////////////////////////////////////////////////

template<typename OutType, typename ComputeType, typename dummy = OutType>
struct model_t;

/**
 * Generic (abstract) model for continous output - regression
 */
template<typename OutType, typename ComputeType>
struct model_t<OutType, ComputeType,
 typename std::enable_if<std::is_floating_point<OutType>::value, OutType>::type>
{

    virtual void predict(base::direction_t direction_XT,
        const El::DistMatrix<ComputeType> &XT, El::DistMatrix<OutType> &YP)
        const = 0;

    virtual boost::property_tree::ptree to_ptree() const = 0;

    virtual void save(const std::string& fname, const std::string& header)
        const = 0;

    virtual El::Int get_input_size() const = 0;

    virtual ~model_t() {

    }
};

/**
 * Generic (abstract) model for discrete (all other) output - classification
 */
template<typename OutType, typename ComputeType>
struct model_t<OutType, ComputeType,
 typename std::enable_if<!std::is_floating_point<OutType>::value, OutType>::type>
{

    virtual void predict(base::direction_t direction_XT,
        const El::DistMatrix<ComputeType> &XT, El::DistMatrix<OutType> &LP,
        El::DistMatrix<ComputeType> &DV)
        const = 0;

    virtual boost::property_tree::ptree to_ptree() const = 0;

    virtual void save(const std::string& fname, const std::string& header)
        const = 0;

    virtual El::Int get_input_size() const = 0;

    virtual void get_column_coding(std::vector<OutType> &rcoding) const = 0;

    virtual ~model_t() {

    }
};

/******************************************************************************/

/**
 * Kernel model.
 */
template<typename KernelType, typename OutType, typename ComputeType = OutType,
         typename dummy = OutType>
struct kernel_model_t;

/**
 * Kernel model for continuous output - regression model
 */
template<typename KernelType, typename OutType, typename ComputeType>
struct kernel_model_t<KernelType, OutType, ComputeType,
  typename std::enable_if<std::is_floating_point<OutType>::value, OutType>::type > :
public model_t<OutType, ComputeType>

{
    typedef KernelType kernel_type;
    typedef ComputeType compute_type;
    typedef OutType out_type;

    kernel_model_t(const kernel_type &k,
        base::direction_t direction, const El::DistMatrix<compute_type> &X,
        const std::string &dataloc, El::Int partial,
        const utility::io::fileformat_t fileformat,
        sketch::generic_sketch_container_t pretransform,
        const El::DistMatrix<compute_type> &A) :
        _X(), _direction(direction),
        _A(), _dataloc(dataloc), _partial(partial),
        _fileformat(fileformat), _pretransform(pretransform), _k(k),
        _input_size(k.get_dim()), _output_size(A.Width()) {

        El::LockedView(_X, X);
        El::LockedView(_A, A);
    }

    kernel_model_t(const boost::property_tree::ptree &pt) {
        build_from_ptree(pt);
    }

    void predict(base::direction_t direction_XT,
        const El::DistMatrix<compute_type> &XT, El::DistMatrix<out_type> &YP) const {

        El::DistMatrix<compute_type> KT;
        Gram(_direction, direction_XT, _k, _X, XT, KT);
        YP.Resize(_A.Height(), KT.Width());
        El::Gemm(El::ADJOINT, El::NORMAL, out_type(1.0), _A, KT, YP);
    }

    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;

        pt.put("skylark_object_type", "model:kernel");
        pt.put("skylark_version", VERSION);

        pt.put("data_location", _dataloc);
        pt.put("partial", _partial);
        pt.put("fileformat", _fileformat);
        if (!_pretransform.empty())
            pt.add_child("pre_transform", _pretransform.to_ptree());
        pt.put("num_outputs", _output_size);
        pt.put("input_size", _input_size);
        pt.put("regression", true);

        pt.add_child("kernel", _k.to_ptree());

        std::stringstream sA;
        El::Print(_A, "", sA);
        pt.put("alpha", sA.str());

        return pt;
    }

    void save(const std::string& fname, const std::string& header) const {
        boost::property_tree::ptree pt = to_ptree();
        std::ofstream of(fname);
        of << header;
        boost::property_tree::write_json(of, pt);
        of.close();
    }

    virtual ~kernel_model_t() {

    }

    El::Int get_input_size() const {
        return _input_size;
    }

protected:
    void build_from_ptree(const boost::property_tree::ptree &pt) {

        _input_size = pt.get<El::Int>("input_size");
        _output_size = pt.get<El::Int>("num_outputs");

        _k = kernel_container_t(pt.get_child("kernel"));
        _dataloc = pt.get<std::string>("data_location");
        _fileformat = (utility::io::fileformat_t)pt.get<int>("fileformat");
        _partial = pt.get<El::Int>("partial");

        // TODO handle "partial" and "sampling"
        El::DistMatrix<OutType> dummyY;

        switch (_fileformat) {
        case utility::io::FORMAT_LIBSVM:
            utility::io::ReadLIBSVM(_dataloc, _X0, dummyY, base::COLUMNS,
                _input_size, _partial);
            break;

#ifdef SKYLARK_HAVE_HDF5
        case utility::io::FORMAT_HDF5: {
            H5::H5File in(_dataloc, H5F_ACC_RDONLY);
            utility::io::ReadHDF5(in, "X", _X0, -1, _partial);
            in.close();
        }
            break;
#endif

        default:
            // TODO
            return;
        }

       if (pt.count("pre_transform") > 0) {
            _pretransform =
                sketch::generic_sketch_container_t::from_ptree(pt.
                    get_child("pre_transform"));

            _X.Resize(_X0.Height(), _pretransform.get_S());
            _pretransform.apply(&_X0, &_X, sketch::rowwise_tag());
            _X0.Empty();
        } else
            El::View(_X, _X0);

        _direction = base::COLUMNS;

        std::istringstream A_str(pt.get<std::string>("alpha"));
        if (_A.Grid().Size() == 1) {
            _A.Resize(_X.Width(), _output_size);
            compute_type *buffer = _A.Buffer();
            int ldim = _A.LDim();
            for(int i = 0; i < _X.Width(); i++) {
                std::string line;
                std::getline(A_str, line);
                std::istringstream Astream(line);
                for(int j = 0; j < _output_size; j++) {
                    std::string token;
                    Astream >> token;
                    buffer[i + j * ldim] = atof(token.c_str());
                }
            }
        } else {
            // TODO: can do more memory efficient
            El::DistMatrix<compute_type, El::CIRC, El::CIRC> A0(_X.Width(),
                _output_size);
            if (A0.Grid().Rank() == 0) {
                compute_type *buffer = A0.Buffer();
                int ldim = A0.LDim();
                for(int i = 0; i < _X.Width(); i++) {
                    std::string line;
                    std::getline(A_str, line);
                    std::istringstream Astream(line);
                    for(int j = 0; j < _output_size; j++) {
                        std::string token;
                        Astream >> token;
                        buffer[i + j * ldim] = atof(token.c_str());
                    }
                }
            }

            _A = A0;
        }
    }

private:
    El::DistMatrix<compute_type> _X, _X0;  // X0 is only for load.
    base::direction_t _direction;
    El::DistMatrix<compute_type> _A;
    std::string _dataloc;
    El::Int _partial;
    utility::io::fileformat_t _fileformat;
    sketch::generic_sketch_container_t _pretransform;
    kernel_type _k;
    El::Int _input_size, _output_size;
};

/**
 * Kernel model for discrete (all other) outputs - classification
 */
template<typename KernelType, typename OutType, typename ComputeType>
struct kernel_model_t<KernelType, OutType, ComputeType,
  typename std::enable_if<!std::is_floating_point<OutType>::value, OutType>::type > :
public model_t<OutType, ComputeType> {

    typedef KernelType kernel_type;
    typedef ComputeType compute_type;
    typedef OutType out_type;

    kernel_model_t(const kernel_type &k,
        base::direction_t direction, const El::DistMatrix<compute_type> &X,
        const std::string &dataloc, El::Int partial,
        const utility::io::fileformat_t fileformat,
        const skylark::sketch::generic_sketch_container_t pretransform,
        const El::DistMatrix<compute_type> &A,
        const std::vector<OutType> &rcoding) :
        _X(), _direction(direction),
        _A(), _rcoding(rcoding), _dataloc(dataloc), _partial(partial),
        _fileformat(fileformat), _pretransform(pretransform),
        _k(k), _input_size(k.get_dim()), _output_size(A.Width()) {

        El::LockedView(_X, X);
        El::LockedView(_A, A);
    }

    kernel_model_t(const boost::property_tree::ptree &pt) {
        build_from_ptree(pt);
    }

    void predict(base::direction_t direction_XT,
        const El::DistMatrix<compute_type> &XT, El::DistMatrix<out_type> &LP,
        El::DistMatrix<compute_type> &DV) const {

        El::DistMatrix<compute_type> KT;
        Gram(_direction, direction_XT, _k, _X, XT, KT);
        El::Gemm(El::ADJOINT, El::NORMAL, compute_type(1.0), _A, KT, DV);
        DummyDecode(El::ADJOINT, DV, LP, _rcoding);
    }

    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;

        pt.put("skylark_object_type", "model:kernel");
        pt.put("skylark_version", VERSION);

        pt.put("data_location", _dataloc);
        pt.put("partial", _partial);
        pt.put("fileformat", _fileformat);
        if (!_pretransform.empty())
            pt.add_child("pre_transform", _pretransform.to_ptree());
        pt.put("num_outputs", _output_size);
        pt.put("input_size", _input_size);
        pt.put("regression", false);

        boost::property_tree::ptree rcoding;
        for(int i = 0; i < _rcoding.size(); i++)
            rcoding.put(std::to_string(i), _rcoding[i]);
        pt.add_child("rcoding", rcoding);

        pt.add_child("kernel", _k.to_ptree());

        std::stringstream sA;
        El::Print(_A, "", sA);
        pt.put("alpha", sA.str());

        return pt;
    }

    void save(const std::string& fname, const std::string& header) const {
        boost::property_tree::ptree pt = to_ptree();
        std::ofstream of(fname);
        of << header;
        boost::property_tree::write_json(of, pt);
        of.close();
    }

    virtual ~kernel_model_t() {

    }

    El::Int get_input_size() const {
        return _input_size;
    }

    void get_column_coding(std::vector<OutType> &rcoding) const {
        rcoding.resize(_rcoding.size());
        for(int i = 0; i < _rcoding.size(); i++)
            rcoding[i] = _rcoding[i];
    }

protected:

    void build_from_ptree(const boost::property_tree::ptree &pt) {

        _input_size = pt.get<El::Int>("input_size");
        _output_size = pt.get<El::Int>("num_outputs");
        _rcoding.resize(_output_size);
        const boost::property_tree::ptree &ptrcoding =
            pt.get_child("rcoding");
        for(El::Int i = 0; i < _output_size; i++)
            _rcoding[i] = ptrcoding.get<OutType>(std::to_string(i));
        _k = kernel_container_t(pt.get_child("kernel"));

        _dataloc = pt.get<std::string>("data_location");
        _fileformat = (utility::io::fileformat_t)pt.get<int>("fileformat");
        _partial = pt.get<int>("partial");

        El::DistMatrix<OutType> dummyL;

        switch (_fileformat) {
        case utility::io::FORMAT_LIBSVM:
            utility::io::ReadLIBSVM(_dataloc, _X0, dummyL, base::COLUMNS,
                _input_size, _partial);
            break;

#ifdef SKYLARK_HAVE_HDF5
        case utility::io::FORMAT_HDF5: {
            H5::H5File in(_dataloc, H5F_ACC_RDONLY);
            utility::io::ReadHDF5(in, "X", _X0, -1, _partial);
            in.close();
        }
            break;
#endif

        default:
            // TODO
            return;
        }

        if (pt.count("pre_transform") > 0) {
            _pretransform =
                sketch::generic_sketch_container_t::from_ptree(pt.
                    get_child("pre_transform"));

            _X.Resize(_X0.Height(), _pretransform.get_S());
            _pretransform.apply(&_X0, &_X, sketch::rowwise_tag());
            _X0.Empty();
        } else
            El::View(_X, _X0);

        _direction = base::COLUMNS;

        std::istringstream A_str(pt.get<std::string>("alpha"));
        if (_A.Grid().Size() == 1) {
            _A.Resize(_X.Width(), _output_size);
            compute_type *buffer = _A.Buffer();
            int ldim = _A.LDim();
            for(int i = 0; i < _X.Width(); i++) {
                std::string line;
                std::getline(A_str, line);
                std::istringstream Astream(line);
                for(int j = 0; j < _output_size; j++) {
                    std::string token;
                    Astream >> token;
                    buffer[i + j * ldim] = atof(token.c_str());
                }
            }
        } else {
            // TODO: can do more memory efficient
            El::DistMatrix<compute_type, El::CIRC, El::CIRC> A0(_X.Width(),
                _output_size);
            if (A0.Grid().Rank() == 0) {
                compute_type *buffer = A0.Buffer();
                int ldim = A0.LDim();
                for(int i = 0; i < _X.Width(); i++) {
                    std::string line;
                    std::getline(A_str, line);
                    std::istringstream Astream(line);
                    for(int j = 0; j < _output_size; j++) {
                        std::string token;
                        Astream >> token;
                        buffer[i + j * ldim] = atof(token.c_str());
                    }
                }
            }

            _A = A0;
        }
    }

private:
    El::DistMatrix<compute_type> _X, _X0; // X0 is only for model load.
    base::direction_t _direction;
    El::DistMatrix<compute_type> _A;
    std::vector<OutType> _rcoding;
    std::string _dataloc;
    El::Int _partial;
    utility::io::fileformat_t _fileformat;
    sketch::generic_sketch_container_t _pretransform;
    kernel_type _k;
    El::Int _input_size, _output_size;
};

/******************************************************************************/

/**
 * Feature expansion model - expands feature expansion and then uses
 * linear combination.
 */
template<template <typename, typename> class SketchType,
         typename OutType, typename ComputeType = OutType,
         typename dummy = OutType>
struct feature_expansion_model_t;

/**
 * Feature expansion model for continuous output - regression model
 */
template<template <typename, typename> class SketchType,
         typename OutType, typename ComputeType>
struct feature_expansion_model_t<SketchType, OutType, ComputeType,
  typename std::enable_if<std::is_floating_point<OutType>::value, OutType>::type > :
public model_t<OutType, ComputeType>

{
    typedef ComputeType compute_type;
    typedef OutType out_type;

    typedef SketchType<El::DistMatrix<compute_type>,
                       El::DistMatrix<compute_type> > sketch_type;

    feature_expansion_model_t(const sketch_type &S,
        const El::DistMatrix<compute_type> &W) :
        _W(),  _scale_maps(false), _feature_transforms(1),
        _input_size(S.get_N()), _output_size(W.Width()),
        _feature_size(S.get_S()) {

       _feature_transforms[0] = S;
        El::LockedView(_W, W);
    }

    feature_expansion_model_t(bool scale_maps,
        const std::vector<sketch_type> &transforms,
        const El::DistMatrix<compute_type> &W) :
        _W(), _scale_maps(scale_maps),
        _feature_transforms(transforms),
        _input_size(_feature_transforms[0].get_N()), _output_size(W.Width()),
        _feature_size(0) {

        for(auto it = _feature_transforms.begin();
            it != _feature_transforms.end(); it++)
            _feature_size += it->get_S();
        El::LockedView(_W, W);
    }

    feature_expansion_model_t(const boost::property_tree::ptree &pt) {
        build_from_ptree(pt);
    }

    void predict(base::direction_t direction_XT,
        const El::DistMatrix<compute_type> &XT, El::DistMatrix<out_type> &YP) const {

        if (direction_XT == base::COLUMNS) {
            El::Zeros(YP, _output_size, XT.Width());
            El::DistMatrix<compute_type> ZT, VW;
            El::Int starts = 0;
            for(int i = 0; i < _feature_transforms.size(); i++) {
                const sketch_type &S = _feature_transforms[i];
                ZT.Resize(S.get_S(), XT.Width());
                S.apply(XT, ZT, sketch::columnwise_tag());
                if (_scale_maps)
                    El::Scale<compute_type, compute_type>
                        (sqrt(double(S.get_S()) / _feature_size), ZT);
                base::RowView(VW, _W, starts, S.get_S());
                starts += S.get_S();
                El::Gemm(El::ADJOINT, El::NORMAL, compute_type(1.0), VW, ZT,
                    compute_type(1.0), YP);
            }

        } else {
            El::Zeros(YP, XT.Height(), _output_size);
            El::DistMatrix<compute_type> ZT, VW;
            El::Int starts = 0;
            for(int i = 0; i < _feature_transforms.size(); i++) {
                const sketch_type &S = _feature_transforms[i];
                S.apply(XT, ZT, sketch::rowwise_tag());
                if (_scale_maps)
                    El::Scale<compute_type, compute_type>
                        (sqrt(double(S.get_S()) / _feature_size), ZT);
                base::RowView(VW, _W, starts, S.get_S());
                starts += S.get_S();
                El::Gemm(El::NORMAL, El::NORMAL, compute_type(1.0), ZT, VW,
                    compute_type(1.0), YP);
            }
        }
    }

    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;

        pt.put("skylark_object_type", "model:feature_expansion");
        pt.put("skylark_version", VERSION);

        pt.put("num_outputs", _output_size);
        pt.put("input_size", _input_size);
        pt.put("regression", true);

        boost::property_tree::ptree ptfmap;
        ptfmap.put("number_transforms", _feature_transforms.size());
        ptfmap.put("scale_maps", _scale_maps);

        boost::property_tree::ptree ptmaps;
        for(El::Int i = 0; i < _feature_transforms.size(); i++)
            ptmaps.push_back(std::make_pair(std::to_string(i),
                    _feature_transforms[i].to_ptree()));
        ptfmap.add_child("transforms", ptmaps);

        pt.add_child("feature_mapping", ptfmap);

        std::stringstream sW;
        El::Print(_W, "", sW);
        pt.put("weights", sW.str());

        return pt;
    }

    void save(const std::string& fname, const std::string& header) const {
        boost::property_tree::ptree pt = to_ptree();
        std::ofstream of(fname);
        of << header;
        boost::property_tree::write_json(of, pt);
        of.close();
    }

    virtual ~feature_expansion_model_t() {

    }

    El::Int get_input_size() const {
        return _input_size;
    }

protected:
    void build_from_ptree(const boost::property_tree::ptree &pt) {

        _input_size = pt.get<El::Int>("input_size");
        _output_size = pt.get<El::Int>("num_outputs");

        int num_transforms = pt.get<int>("feature_mapping.number_transforms");
        _scale_maps = pt.get<bool>("feature_mapping.scale_maps");

        El::Int s = 0;
        _feature_transforms.resize(num_transforms);
        const boost::property_tree::ptree &ptmaps =
            pt.get_child("feature_mapping.transforms");
        for(int i = 0; i < num_transforms; i++) {
            _feature_transforms[i] =
                sketch_type(sketch_type::from_ptree(ptmaps.get_child(std::to_string(i))));
            s += _feature_transforms[i].get_S();
        }

        std::istringstream W_str(pt.get<std::string>("weights"));
        if (_W.Grid().Size() == 1) {
            _W.Resize(s, _output_size);
            compute_type *buffer = _W.Buffer();
            int ldim = _W.LDim();
            for(int i = 0; i < s; i++) {
                std::string line;
                std::getline(W_str, line);
                std::istringstream Wstream(line);
                for(int j = 0; j < _output_size; j++) {
                    std::string token;
                    Wstream >> token;
                    buffer[i + j * ldim] = atof(token.c_str());
                }
            }
        } else {
            // TODO: can do more memory efficient
            El::DistMatrix<compute_type, El::CIRC, El::CIRC> W0(s, _output_size);
            if (W0.Grid().Rank() == 0) {
                compute_type *buffer = W0.Buffer();
                int ldim = W0.LDim();
                for(int i = 0; i < s; i++) {
                    std::string line;
                    std::getline(W_str, line);
                    std::istringstream Wstream(line);
                    for(int j = 0; j < _output_size; j++) {
                        std::string token;
                        Wstream >> token;
                        buffer[i + j * ldim] = atof(token.c_str());
                    }
                }
            }

            _W = W0;
        }
    }

private:
    El::DistMatrix<compute_type> _W;
    bool _scale_maps;
    std::vector<sketch_type> _feature_transforms;
    El::Int _input_size, _output_size;
    El::Int _feature_size;
};

/**
 * Approximate kernel model for discrete (all other) outputs - classification
 */
template<template <typename, typename> class SketchType,
         typename OutType, typename ComputeType>
struct feature_expansion_model_t<SketchType, OutType, ComputeType,
  typename std::enable_if<!std::is_floating_point<OutType>::value, OutType>::type > :
public model_t<OutType, ComputeType> {

    typedef ComputeType compute_type;
    typedef OutType out_type;

    typedef SketchType<El::DistMatrix<compute_type>,
                       El::DistMatrix<compute_type> > sketch_type;

    feature_expansion_model_t(const sketch_type &S,
        const El::DistMatrix<compute_type> &W,
        const std::vector<OutType> &rcoding) :
        _W(), _rcoding(rcoding), _scale_maps(false), _feature_transforms(1),
        _input_size(S.get_N()), _output_size(W.Width()),
        _feature_size(S.get_S()) {

        _feature_transforms[0] = S;
        El::LockedView(_W, W);
    }

    feature_expansion_model_t(bool scale_maps,
        const std::vector<sketch_type> &transforms,
        const El::DistMatrix<compute_type> &W,
        const std::vector<OutType> &rcoding) :
        _W(), _rcoding(rcoding), _scale_maps(scale_maps),
        _feature_transforms(transforms),
        _input_size(_feature_transforms[0].get_N()), _output_size(W.Width()),
        _feature_size(0) {

        for(auto it = _feature_transforms.begin();
            it != _feature_transforms.end(); it++)
            _feature_size += it->get_S();
        El::LockedView(_W, W);
    }

    feature_expansion_model_t(const boost::property_tree::ptree &pt) {
        build_from_ptree(pt);
    }

    void predict(base::direction_t direction_XT,
        const El::DistMatrix<compute_type> &XT, El::DistMatrix<out_type> &LP,
        El::DistMatrix<compute_type> &DV) const {

        if (direction_XT == base::COLUMNS) {

            El::DistMatrix<compute_type> ZT, VW;
            El::Zeros(DV, _output_size, XT.Width());
            El::Int starts = 0;
            for(int i = 0; i < _feature_transforms.size(); i++) {
                const sketch_type &S = _feature_transforms[i];
                ZT.Resize(S.get_S(), XT.Width());
                S.apply(XT, ZT, sketch::columnwise_tag());
                if (_scale_maps)
                    El::Scale<compute_type, compute_type>
                        (sqrt(double(S.get_S()) / _feature_size), ZT);
                base::RowView(VW, _W, starts, S.get_S());
                starts += S.get_S();
                El::Gemm(El::ADJOINT, El::NORMAL, compute_type(1.0), VW, ZT,
                    compute_type(1.0), DV);
            }
            DummyDecode(El::ADJOINT, DV, LP, _rcoding);

        } else {

            El::DistMatrix<compute_type> ZT, VW;
            El::Zeros(DV, XT.Height(), _output_size);
            El::Int starts = 0;
            for(int i = 0; i < _feature_transforms.size(); i++) {
                const sketch_type &S = _feature_transforms[i];
                S.apply(XT, ZT, sketch::rowwise_tag());
                if (_scale_maps)
                    El::Scale<compute_type, compute_type>
                        (sqrt(double(S.get_S()) / _feature_size), ZT);
                base::RowView(VW, _W, starts, S.get_S());
                starts += S.get_S();
                El::Gemm(El::NORMAL, El::NORMAL, compute_type(1.0), ZT, VW,
                    compute_type(1.0), DV);
            }
            DummyDecode(El::NORMAL, DV, LP, _rcoding);

        }
    }

    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;

        pt.put("skylark_object_type", "model:feature_expansion");
        pt.put("skylark_version", VERSION);

        pt.put("input_size", _input_size);
        pt.put("num_outputs", _output_size);
        pt.put("regression", false);

        boost::property_tree::ptree rcoding;
        for(int i = 0; i < _rcoding.size(); i++)
            rcoding.put(std::to_string(i), _rcoding[i]);
        pt.add_child("rcoding", rcoding);

        boost::property_tree::ptree ptfmap;
        ptfmap.put("number_transforms", _feature_transforms.size());
        ptfmap.put("scale_maps", _scale_maps);

        boost::property_tree::ptree ptmaps;
        for(El::Int i = 0; i < _feature_transforms.size(); i++)
            ptmaps.push_back(std::make_pair(std::to_string(i),
                    _feature_transforms[i].to_ptree()));
        ptfmap.add_child("transforms", ptmaps);
        pt.add_child("feature_mapping", ptfmap);

        std::stringstream sW;
        El::Print(_W, "", sW);
        pt.put("weights", sW.str());

        return pt;
    }

    void save(const std::string& fname, const std::string& header) const {
        boost::property_tree::ptree pt = to_ptree();
        std::ofstream of(fname);
        of << header;
        boost::property_tree::write_json(of, pt);
        of.close();
    }

    virtual ~feature_expansion_model_t() {

    }

    El::Int get_input_size() const {
        return _input_size;
    }

    void get_column_coding(std::vector<OutType> &rcoding) const {
        rcoding.resize(_rcoding.size());
        for(int i = 0; i < _rcoding.size(); i++)
            rcoding[i] = _rcoding[i];
    }

protected:
    void build_from_ptree(const boost::property_tree::ptree &pt) {

        _input_size = pt.get<El::Int>("input_size");
        _output_size = pt.get<El::Int>("num_outputs");
        _rcoding.resize(_output_size);
        const boost::property_tree::ptree &ptrcoding =
            pt.get_child("rcoding");
        for(El::Int i = 0; i < _output_size; i++)
            _rcoding[i] = ptrcoding.get<OutType>(std::to_string(i));

        int num_transforms = pt.get<int>("feature_mapping.number_transforms");
        _scale_maps = pt.get<bool>("feature_mapping.scale_maps");

        El::Int s = 0;
        _feature_transforms.resize(num_transforms);
        const boost::property_tree::ptree &ptmaps =
            pt.get_child("feature_mapping.transforms");
        for(int i = 0; i < num_transforms; i++) {
            _feature_transforms[i] =
                sketch_type(sketch_type::from_ptree(ptmaps.get_child(std::to_string(i))));
            s += _feature_transforms[i].get_S();
        }

        std::istringstream W_str(pt.get<std::string>("weights"));
        if (_W.Grid().Size() == 1) {
            _W.Resize(s, _output_size);
            compute_type *buffer = _W.Buffer();
            int ldim = _W.LDim();
            for(int i = 0; i < s; i++) {
                std::string line;
                std::getline(W_str, line);
                std::istringstream Wstream(line);
                for(int j = 0; j < _output_size; j++) {
                    std::string token;
                    Wstream >> token;
                    buffer[i + j * ldim] = atof(token.c_str());
                }
            }
        } else {
            // TODO: can do more memory efficient
            El::DistMatrix<compute_type, El::CIRC, El::CIRC> W0(s, _output_size);
            if (W0.Grid().Rank() == 0) {
                compute_type *buffer = W0.Buffer();
                int ldim = W0.LDim();
                for(int i = 0; i < s; i++) {
                    std::string line;
                    std::getline(W_str, line);
                    std::istringstream Wstream(line);
                    for(int j = 0; j < _output_size; j++) {
                        std::string token;
                        Wstream >> token;
                        buffer[i + j * ldim] = atof(token.c_str());
                    }
                }
            }

            _W = W0;
        }
    }

private:
    El::DistMatrix<compute_type> _W;
    std::vector<OutType> _rcoding;
    bool _scale_maps;
    std::vector<sketch_type> _feature_transforms;
    El::Int _input_size, _output_size;
    El::Int _feature_size;
};

/******************************************************************************/

/**
 * Container -
 * Generic (abstract) model.
 */
template<typename OutType, typename ComputeType = OutType,
         typename dummy = OutType>
struct model_container_t;

/**
 * Container -
 * Generic (abstract) model for continious output - regression
 */
template<typename OutType, typename ComputeType>
struct model_container_t<OutType, ComputeType,
  typename std::enable_if<std::is_floating_point<OutType>::value, OutType>::type> :
public model_t<OutType, ComputeType>
{
    typedef model_t<OutType, ComputeType> model_type;

    model_container_t(const std::shared_ptr<model_type> m) :
        _m(m) {
    }

    model_container_t(const boost::property_tree::ptree &pt) {
        std::string type = pt.get<std::string>("skylark_object_type");

        if (type == "model:kernel")
            _m.reset(new kernel_model_t<skylark::ml::kernel_container_t,
                OutType, ComputeType>(pt));

        if (type == "model:feature_expansion")
            _m.reset(new feature_expansion_model_t<
                sketch::sketch_transform_container_t,
                OutType, ComputeType>(pt));
    }

    virtual void predict(base::direction_t direction_XT,
        const El::DistMatrix<ComputeType> &XT, El::DistMatrix<OutType> &YP) const {
        _m->predict(direction_XT, XT, YP);
    }

    virtual boost::property_tree::ptree to_ptree() const {
        return _m->to_ptree();
    }

    virtual void save(const std::string& fname, const std::string& header)
        const {
        _m->save(fname, header);
    }

    virtual El::Int get_input_size() const {
        return _m->get_input_size();
    }

    virtual ~model_container_t() {

    }

private:
    std::shared_ptr<model_type> _m;
};

/**
 * Container -
 * Generic (abstract) model for discrete (all other) output - classification
 */
template<typename OutType, typename ComputeType>
struct model_container_t<OutType, ComputeType,
  typename std::enable_if<!std::is_floating_point<OutType>::value, OutType>::type> :
public model_t<OutType, ComputeType>
{
    typedef model_t<OutType, ComputeType> model_type;

    model_container_t(const std::shared_ptr<model_type> m) :
        _m(m) {
    }

    model_container_t(const boost::property_tree::ptree &pt) {
        std::string type = pt.get<std::string>("skylark_object_type");

        if (type == "model:kernel")
            _m.reset(new kernel_model_t<skylark::ml::kernel_container_t,
                OutType, ComputeType>(pt));

        if (type == "model:feature_expansion")
            _m.reset(new feature_expansion_model_t<
                sketch::sketch_transform_container_t,
                OutType, ComputeType>(pt));
    }

    virtual void predict(base::direction_t direction_XT,
        const El::DistMatrix<ComputeType> &XT, El::DistMatrix<OutType> &LP,
        El::DistMatrix<ComputeType> &DV) const {
        _m->predict(direction_XT, XT, LP, DV);
    }

    virtual boost::property_tree::ptree to_ptree() const {
        return _m->to_ptree();
    }

    virtual void save(const std::string& fname, const std::string& header)
        const {
        _m->save(fname, header);
    }

    virtual El::Int get_input_size() const {
        return _m->get_input_size();
    }

    virtual void get_column_coding(std::vector<OutType> &rcoding) const {
        return _m->get_column_coding(rcoding);
    }

    virtual ~model_container_t() {

    }

private:
    std::shared_ptr<model_type> _m;
};

} }

#endif /* SKYLARK_ML_MODEL_HPP */


#include <iostream>
#include <boost/date_time/gregorian/gregorian.hpp>

using namespace std;
using namespace boost::gregorian;

int main(){
    //  There's shitf() and expand() in date_period
    //      date_period.shift()     将区间平移n天而长度不变
    //      date_period.expand()    将区间向两边延伸n天
    date_period dp(date(2014,1,2),days(20));    //  从2014-1-1开始的20天
    dp.shift(days(3));              //  From 2014-1-4 to 2014-1-24
    cout << "dp's begin() = " << dp.begin() << "  end() = " << dp.end();
    cout << "It lasts " << dp.length().days() << " days" << endl;

    dp.expand(days(3));
    cout << "After date_preiod.expand(days(3)) ------" << endl;
    cout << "dp's begin() = " << dp.begin() << "  end() = " << dp.end();
    cout << "  It lasts " << dp.length().days() << " days" << endl;

    // date_period.is_before(date)/is_after(date)
    // date_period.contains(date)
    // date_period.intersects(date)             //  是否存在交集
    // date_period.is_adjacent(date_period)     //  是否相邻

    date_period dp2(date(2014,2,1),days(30));
    if (dp.is_before(date(2014,1,1)))
        cout << "dp is before 2014-1-1" << endl;
    else if(dp.is_after(date(2014,1,1)))
        cout << "dp is after 2014-1-1" << endl;
    else if(dp.contains(date(2014,1,1)))
        cout << "dp contains 2014-1-1" << endl;
    else
        cout << "What's up?" << endl;

    // date_period.merge(date_period)   // 返回两个date_period的并集
    //                                      如果两个date_period无交集且不相邻
    //                                      返回无效区间
    // date_period.span(date_period)    // 合并两日期区间的间隔，相当于广义的merge()

    return 0;
}


#define NDEBUG
#include "ETL.h"

#include <vector>
#include <string>
#include <stdlib.h>
#include <boost/algorithm/string.hpp>

using namespace std;

vector<vector<string>> ETL::readCSV() {
	ifstream file (dataset);
	vector<vector<string>> dataString;

	string line = "";

	while(getline(file, line)){
		vector <string> vec;
		boost::algorithm::split(vec, line, boost::is_any_of(delimiter));
		dataString.push_back(vec);
	}
	file.close();

	return dataString;
}

Eigen::MatrixXd ETL::CSVtoEigen(vector<vector<string>> dataset, int rows, int cols){
	if (header == true) {
	  rows  = rows - 1;
	}
	Eigen::MatrixXd mat(cols, rows);
	for (int i =0; i<rows; i++){
		for (int j=0; j <cols;  ++j){
			mat(j,i) = atof(dataset[i][j].c_str());
		}
	}
	return mat.transpose();
}


// UniformDistribution.hpp
//
// (C) Datasim Education BV  2009

#pragma once

#include <boost/math/distributions.hpp>

using namespace System;

namespace Wrapper
{

	// Wrapper for the boost::math::uniform_distribution class
	// We use the .NET naming conventions instead of the original C++ name
	public ref class UniformDistribution
	{
	private:
		// The wrapped native class (only pointers to native classes can be a C++/CLI class datamember)
		boost::math::uniform_distribution<>* m_distribution;

	public:
		// Default constructor
		UniformDistribution();

		// Constructor with lower and upper value
		UniformDistribution(double lower, double upper);

		// Finaliser (called by garbage collector or destructor)
		!UniformDistribution();

		// Destructor (Dispose)
		~UniformDistribution();

		// Access the lower value
		property double Lower
		{
			// Get the lower value
			double get();
		}

		// Access the upper value
		property double Upper
		{
			// Get the upper value
			double get();
		}

		// Get the native object
		boost::math::uniform_distribution<>* GetNative();
	};
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/Wrapper/UniformDistribution.hpp


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/exponent.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/if_nan_else.hpp>
#include <boost/simd/function/if_plus.hpp>
#include <boost/simd/function/is_invalid.hpp>
#include <boost/simd/constant/mindenormal.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>
#include <boost/simd/constant/smallestposval.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( eps_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_< bd::integer_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator()(A0 const&) const BOOST_NOEXCEPT
    {
      return A0(1);
    }
  };

  BOOST_DISPATCH_OVERLOAD_IF ( eps_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::floating_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a00) const BOOST_NOEXCEPT
    {
      using iA0 = bd::as_integer_t<A0>;
      using sA0 = typename A0::value_type;

      A0    a = boost::simd::abs(a00);
      auto e1 = exponent(a)-Nbmantissabits<A0>();
      A0    e =  bitwise_cast<A0>(bitwise_cast<iA0>(A0(1))+(shift_left(e1,Nbmantissabits<sA0>())));

#ifndef BOOST_SIMD_NO_DENORMALS
      return if_plus( is_invalid(a)
                    , if_else(a < Smallestposval<A0>(), Mindenormal<A0>(), e)
                    , Nan<A0>()
                    );
#else
      return if_nan_else(is_invalid(a), e);
#endif
    }
  };
} } }

#endif


#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

namespace bear {
void quaternion_to_sh_rotation_matrix(const Eigen::Quaterniond &q,
                                      int order,
                                      Eigen::Ref<Eigen::MatrixXd> sh_mat);
}  // namespace bear


/*    Copyright (c) 2010-2018, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *    Hackworth, M. Magnetic Fields and Magnetic Forces,
 *          http://www2.cose.isu.edu/~hackmart/Magnetic%20Fields_engphys.pdf, last accessed: 20th
 *          January, 2014.
 *    Khan, S. Magnetism 3 Whats happens when a speeding proton goes through a magnetic field,
 *          https://www.khanacademy.org/science/physics/electricity-and-magnetism/v/magnetism-3,
 *          2013, last accessed: 16th January, 2014.
 *    Learning About Electronics Magnetic Force Calculator,
 *          http://www.learningaboutelectronics.com/Articles/Magnetic-force-calculator.php#answer1,
 *          last accessed: 20th January, 2014.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

#include <boost/test/unit_test.hpp>

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

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Astrodynamics/ElectroMagnetism/lorentzStaticMagneticForce.h"
#include "Tudat/Astrodynamics/ElectroMagnetism/lorentzStaticMagneticAcceleration.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_lorentz_static_magnetic_acceleration_and_force_models )

//! Test implementation of static magnetic force model.
BOOST_AUTO_TEST_CASE( testStaticMagneticForceKhanAcademy )
{
    // Benchmark data is obtained using online video (Khan, 2013).

    // Tests approximate force on a proton due to a static uniform magnetic field.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticForce
                = Eigen::Vector3d( 0.0, -4.80653199e-12, 0.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 6.0e7, 0.0, 0.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 0.0, 0.0, 0.5 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 1.60217733e-19;

    // Compute static magnetic force [N].
    const Eigen::Vector3d computedStaticMagneticForce
                = electro_magnetism::computeLorentzForceDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic force vectors.
    BOOST_CHECK_SMALL( computedStaticMagneticForce.x( ),
                       std::numeric_limits< double >::min( ) );

    BOOST_CHECK_CLOSE_FRACTION( computedStaticMagneticForce.y( ),
                                expectedStaticMagneticForce.y( ),
                                std::numeric_limits< double >::epsilon( ) );

    BOOST_CHECK_SMALL( computedStaticMagneticForce.z( ),
                       std::numeric_limits< double >::min( ) );
}

//! Test implementation of static magnetic force model.
BOOST_AUTO_TEST_CASE( testStaticMagneticForceArbitraryCharge )
{
    // Benchmark data is obtained using Learning About Electronics.

    // Tests force using arbitrary values for parameters.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticForce = Eigen::Vector3d( 218.0, -172.0, 4.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 4.0, 5.0, -3.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 2.0, 3.0, 20.0 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 2.0;

    // Compute static magnetic force [N].
    const Eigen::Vector3d computedStaticMagneticForce
                = electro_magnetism::computeLorentzForceDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic force vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedStaticMagneticForce,
                                       expectedStaticMagneticForce,
                                       std::numeric_limits< double >::epsilon( ) );
}

//! Test implementation of static magnetic force model.
BOOST_AUTO_TEST_CASE( testStaticMagneticForceHackworth )
{
    // Benchmark data is obtained from pdf (Hackworth, M.).
    // Tests force on charge in uniform magnetic field.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticForce = Eigen::Vector3d( 77.0, 28.0, -10.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( -2.0, 3.0, -7.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 4.0, -11.0, 0.0 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = -1.0;

    // Compute static magnetic force [N].
    const Eigen::Vector3d computedStaticMagneticForce
                = electro_magnetism::computeLorentzForceDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic force vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedStaticMagneticForce,
                                       expectedStaticMagneticForce,
                                       std::numeric_limits< double >::epsilon( ) );
}

//! Test implementation of static magnetic acceleration model.
BOOST_AUTO_TEST_CASE( testStaticMagneticAccelerationKhanAcademy )
{
    // Benchmark data is obtained using online video (Khan, 2013).

    // Tests approximate force on a proton due to a static uniform magnetic field.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticAcceleration =
            Eigen::Vector3d( 0.0, -2.8736514e15, 0.0 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 6.0e7, 0.0, 0.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 0.0, 0.0, 0.5 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 1.60217733e-19;

    // Set mass of accelerated body [kg].
    const double massOfBodySubjectToAcceleration = 1.67262178e-27;

    // Compute static magnetic acceleration [N].
    const Eigen::Vector3d computedStaticMagneticAcceleration
                = electro_magnetism::computeLorentzAccelerationDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration,
                    massOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic acceleration vectors.
    BOOST_CHECK_SMALL( computedStaticMagneticAcceleration.x( ),
                       std::numeric_limits< double >::min( ) );

    BOOST_CHECK_CLOSE_FRACTION( computedStaticMagneticAcceleration.y( ),
                                expectedStaticMagneticAcceleration.y( ),
                                1.0e-7 );

    BOOST_CHECK_SMALL( computedStaticMagneticAcceleration.z( ),
                       std::numeric_limits< double >::min( ) );
}

//! Test implementation of static magnetic acceleration model.
BOOST_AUTO_TEST_CASE( testStaticMagneticAccelerationArbitraryCharge )
{
    // Benchmark data is obtained using Learning About Electronics.

    // Tests force using arbitrary values for parameters.

    // Set expected static magnetic force [N].
    const Eigen::Vector3d expectedStaticMagneticAcceleration
                = Eigen::Vector3d( 43.6, -34.4, 0.8 );

    // Set velocity of accelerated body [m/s].
    const Eigen::Vector3d velocityOfBodySubjectToAcceleration = Eigen::Vector3d( 4.0, 5.0, -3.0 );

    // Set strength of local magnetic field [T].
    const Eigen::Vector3d localMagneticField = Eigen::Vector3d( 2.0, 3.0, 20.0 );

    // Set charge of accelerated body [C].
    const double chargeOfBodySubjectToAcceleration = 2.0;

    // Set mass of accelerated body [kg].
    const double massOfBodySubjectToAcceleration = 5.0;

    // Compute static magnetic acceleration [N].
    const Eigen::Vector3d computedStaticMagneticAcceleration
                = electro_magnetism::computeLorentzAccelerationDueToStaticMagneticField(
                    velocityOfBodySubjectToAcceleration,
                    localMagneticField,
                    chargeOfBodySubjectToAcceleration,
                    massOfBodySubjectToAcceleration );

    // Compare computed and expected static magnetic acceleration vectors.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( computedStaticMagneticAcceleration,
                                       expectedStaticMagneticAcceleration,
                                       std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#ifndef POSE_ESTIMATER_HPP
#define POSE_ESTIMATER_HPP

#include <Eigen/Core>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/plot.hpp>
#include "data.hpp"
#include "stereo_processor/g2o_edges/pose_edge.hpp"

class PoseEstimater {
public:
	double poseEstimate(const FeaturePoints& fts_pts, const cv::Mat& source_img, const cv::Mat& K, const cv::Mat& dest_img, int pymd, int iter, Eigen::Matrix3d& R, Eigen::Vector3d& t);
	void refine_pose(const PointsWithUncertainties& points, const std::vector<cv::Point2f>& features, const cv::Mat& K, Eigen::Matrix3d& R, Eigen::Vector3d& t);

private:
	double poseEstimatePymd(const std::vector<Eigen::Vector3d>& pts, const std::vector<double>& uncertainties, const std::vector<cv::Point2f>& fts,
													const cv::Mat& source_img, const cv::Mat& dest_img, const Eigen::Matrix3d& K, Eigen::Matrix3d& R, Eigen::Vector3d& t, int iter);
};

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/function/interleave_odd.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

STF_CASE_TPL( "interleave_odd based shuffle", STF_NUMERIC_TYPES)
{
  // cardinal 4
  {
    bs::pack<T,4> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>() };
    bs::pack<T,4> b{ T(13), bs::Valmin<T>(), T(37), bs::Valmax<T>() };
    bs::pack<T,4> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 1, 5, 3, 7>(a,b)), (bs::interleave_odd(a,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 5,-1, 7>(a,b)), (bs::interleave_odd(z,b)) );
    STF_ALL_EQUAL( (bs::shuffle< 1,-1, 3,-1>(a,b)), (bs::interleave_odd(a,z)) );

    STF_ALL_EQUAL( (bs::shuffle< 5, 1, 7, 3>(a,b)), (bs::interleave_odd(b,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 1,-1, 3>(a,b)), (bs::interleave_odd(z,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 5,-1, 7,-1>(a,b)), (bs::interleave_odd(b,z)) );
  }

  // cardinal 8
  {
    bs::pack<T,8> a{ bs::Valmax<T>(), T(42), T(69), bs::Valmin<T>(), T(37), bs::Valmax<T>(), T(55), T(99) };
    bs::pack<T,8> b{ T(13), bs::Valmax<T>(), T(42), bs::Valmax<T>(), T(76), bs::Valmin<T>(), T(42), T(69) };
    bs::pack<T,8> z(0);

    STF_ALL_EQUAL( (bs::shuffle< 1, 9, 3,11, 5,13, 7,15>(a,b)), (bs::interleave_odd(a,b)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 9,-1,11,-1,13,-1,15>(a,b)), (bs::interleave_odd(z,b)) );
    STF_ALL_EQUAL( (bs::shuffle< 1,-1, 3,-1, 5,-1, 7,-1>(a,b)), (bs::interleave_odd(a,z)) );

    STF_ALL_EQUAL( (bs::shuffle< 9, 1,11, 3,13, 5,15, 7>(a,b)), (bs::interleave_odd(b,a)) );
    STF_ALL_EQUAL( (bs::shuffle<-1, 1,-1, 3,-1, 5,-1, 7>(a,b)), (bs::interleave_odd(z,a)) );
    STF_ALL_EQUAL( (bs::shuffle< 9,-1,11,-1,13,-1,15,-1>(a,b)), (bs::interleave_odd(b,z)) );
  }
}


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


#include <stdio.h>
#include <vector>
#include <map>
#include <random>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/simple_robot_model_interface.hpp>

#ifndef SIMPLE_SE2_ROBOT_MODEL_HPP
#define SIMPLE_SE2_ROBOT_MODEL_HPP

namespace simple_se2_robot_model
{
    using SimpleSE2Configuration = Eigen::Matrix<double, 3, 1>;
    using SimpleSE2ConfigAlloc = std::allocator<Eigen::Matrix<double, 3, 1>>;

    class SimpleSE2ConfigSerializer
    {
    public:

        static inline std::string TypeName()
        {
            return std::string("EigenMatrixD31Serializer");
        }

        static inline uint64_t Serialize(const SimpleSE2Configuration& value, std::vector<uint8_t>& buffer)
        {
            return EigenHelpers::Serialize(value, buffer);
        }

        static inline std::pair<SimpleSE2Configuration, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
        {
            return EigenHelpers::Deserialize<Eigen::Matrix<double, 3, 1>>(buffer, current);
        }
    };

    class SimpleSE2Robot : public simple_robot_model_interface::SimpleRobotModelInterface<SimpleSE2Configuration, SimpleSE2ConfigAlloc>
    {
    protected:

        Eigen::Isometry3d pose_;
        SimpleSE2Configuration config_;
        double position_distance_weight_;
        double rotation_distance_weight_;
        std::string link_name_;

        inline void SetConfig(const SimpleSE2Configuration& new_config)
        {
            config_(0) = new_config(0);
            config_(1) = new_config(1);
            config_(2) = EigenHelpers::EnforceContinuousRevoluteBounds(new_config(2));
            // Update pose
            pose_ = ComputePose();
        }

        inline Eigen::Isometry3d ComputePose() const
        {
            const SimpleSE2Configuration& current_config = GetPosition();
            const Eigen::Translation3d current_position(current_config(0), current_config(1), 0.0);
            const double current_z_angle = current_config(2);
            const Eigen::Isometry3d z_joint_transform = current_position * Eigen::Quaterniond::Identity();
            const Eigen::Isometry3d body_transform = z_joint_transform * (Eigen::Translation3d::Identity() * Eigen::Quaterniond(Eigen::AngleAxisd(current_z_angle, Eigen::Vector3d::UnitZ())));
            return body_transform;
        }

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        SimpleSE2Robot(const SimpleSE2Configuration& initial_position,
                       const double position_distance_weight,
                       const double rotation_distance_weight,
                       const std::string& link_name)
            : simple_robot_model_interface::SimpleRobotModelInterface<SimpleSE2Configuration, SimpleSE2ConfigAlloc>(),
              position_distance_weight_(std::abs(position_distance_weight)),
              rotation_distance_weight_(std::abs(rotation_distance_weight)),
              link_name_(link_name)
        {
            SetPosition(initial_position);
        }

        virtual simple_robot_model_interface::SimpleRobotModelInterface<SimpleSE2Configuration, SimpleSE2ConfigAlloc>* Clone() const
        {
            return new SimpleSE2Robot(static_cast<const SimpleSE2Robot&>(*this));
        }

        virtual const SimpleSE2Configuration& GetPosition() const
        {
            return config_;
        }

        virtual const SimpleSE2Configuration& SetPosition(const SimpleSE2Configuration& position)
        {
            SetConfig(position);
            return GetPosition();
        }

        virtual std::vector<std::string> GetLinkNames() const
        {
            return std::vector<std::string>{link_name_};
        }

        virtual Eigen::Isometry3d GetLinkTransform(const std::string& link_name) const
        {
            if (link_name == link_name_)
            {
                return pose_;
            }
            else
            {
                throw std::invalid_argument("Invalid link_name");
            }
        }

        virtual EigenHelpers::VectorIsometry3d GetLinkTransforms() const
        {
            return EigenHelpers::VectorIsometry3d(1, pose_);
        }

        virtual EigenHelpers::MapStringIsometry3d GetLinkTransformsMap() const
        {
            EigenHelpers::MapStringIsometry3d link_transforms_map;
            link_transforms_map[link_name_] = pose_;
            return link_transforms_map;
        }

        virtual Eigen::VectorXd ComputePerDimensionConfigurationSignedDistance(const SimpleSE2Configuration& config1, const SimpleSE2Configuration& config2) const
        {
            Eigen::VectorXd dim_distances(3);
            dim_distances(0) = config2(0) - config1(0);
            dim_distances(1) = config2(1) - config1(1);
            dim_distances(2) = EigenHelpers::ContinuousRevoluteSignedDistance(config1(2), config2(2));;
            return dim_distances;
        }

        virtual double ComputeConfigurationDistance(const SimpleSE2Configuration& config1, const SimpleSE2Configuration& config2) const
        {
            const Eigen::VectorXd dim_distances = ComputePerDimensionConfigurationSignedDistance(config1, config2);
            const double trans_dist = sqrt((dim_distances(0) * dim_distances(0)) + (dim_distances(1) * dim_distances(1)));
            const double rots_dist = std::abs(dim_distances(2));
            return (trans_dist * position_distance_weight_) + (rots_dist * rotation_distance_weight_);
        }

        virtual Eigen::Matrix<double, 3, 1> InterpolateBetweenConfigurations(const SimpleSE2Configuration& start, const SimpleSE2Configuration& end, const double ratio) const
        {
            Eigen::Matrix<double, 3, 1> interpolated = Eigen::Matrix<double, 3, 1>::Zero();
            interpolated(0) = EigenHelpers::Interpolate(start(0), end(0), ratio);
            interpolated(1) = EigenHelpers::Interpolate(start(1), end(1), ratio);
            interpolated(2) = EigenHelpers::InterpolateContinuousRevolute(start(2), end(2), ratio);
            return interpolated;
        }

        virtual Eigen::Matrix<double, 3, 1> AverageConfigurations(const std::vector<SimpleSE2Configuration, SimpleSE2ConfigAlloc>& configurations) const
        {
            if (configurations.size() > 0)
            {
                // Separate translation and rotation values
                std::vector<Eigen::VectorXd> translations(configurations.size());
                std::vector<double> zrs(configurations.size());
                for (size_t idx = 0; idx < configurations.size(); idx++)
                {
                    const SimpleSE2Configuration& state = configurations[idx];
                    Eigen::VectorXd trans_state(2);
                    trans_state << state(0), state(1);
                    translations[idx] = trans_state;
                    zrs[idx] = state(2);
                }
                // Get the average values
                const Eigen::VectorXd average_translation = EigenHelpers::AverageEigenVectorXd(translations);
                const double average_zr = EigenHelpers::AverageContinuousRevolute(zrs);
                Eigen::Matrix<double, 3, 1> average;
                average << average_translation, average_zr;
                return average;
            }
            else
            {
                return Eigen::Matrix<double, 3, 1>::Zero();
            }
        }

        virtual Eigen::Matrix<double, 3, Eigen::Dynamic> ComputeLinkPointTranslationJacobian(const std::string& link_name, const Eigen::Vector4d& link_relative_point) const
        {
            if (link_name == link_name_)
            {
                const Eigen::Matrix<double, 3, 1> current_config = GetPosition();
                // Transform the point into world frame
                const Eigen::Isometry3d current_transform = pose_;
                const Eigen::Vector3d current_position(current_config(0), current_config(1), 0.0);
                const Eigen::Vector3d world_point = (current_transform * link_relative_point).block<3, 1>(0, 0);
                // Make the jacobian
                Eigen::Matrix<double, 3, 3> jacobian = Eigen::Matrix<double, 3, 3>::Zero();
                // Prismatic joints
                // X joint
                jacobian.block<3,1>(0, 0) = jacobian.block<3,1>(0, 0) + Eigen::Vector3d::UnitX();
                // Y joint
                jacobian.block<3,1>(0, 1) = jacobian.block<3,1>(0, 1) + Eigen::Vector3d::UnitY();
                // Rotatational joints
                // Compute Z-axis joint axis
                const Eigen::Isometry3d z_joint_transform = current_transform;
                const Eigen::Vector3d z_joint_axis = (Eigen::Vector3d)(z_joint_transform.rotation() * Eigen::Vector3d::UnitZ());
                jacobian.block<3,1>(0, 2) = jacobian.block<3,1>(0, 2) + z_joint_axis.cross(world_point - current_position);
                return jacobian;
            }
            else
            {
                throw std::invalid_argument("Invalid link_name");
            }
        }
    };
}

#endif // SIMPLE_SE2_ROBOT_MODEL_HPP


/**
 * \file src/gps.cc
 * Measurements using GPS.
 */

#include <shg/gps.h>
#include <cmath>
#include <cstring>
#include <cstdlib>
#include <cstdio>
#include <iostream>
#ifdef HAVE_PUGIXML
#include <exception>
#include <fstream>
#include <sstream>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#endif
#include <shg/utils.h>
#include <shg/geometry.h>

namespace SHG::GPS {

namespace {

constexpr double eccentricity_squared =
     (sqr(semi_major_axis) - sqr(semi_minor_axis)) /
     sqr(semi_major_axis);
constexpr double one_minus_eccentricity_squared =
     1.0 - eccentricity_squared;

}  // anonymous namespace

void convert(const Geogr_coord& g, Cart_coord& p) {
     const double phi = degrees_to_radians(g.phi);
     const double lambda = degrees_to_radians(g.lambda);
     const double sin_phi = std::sin(phi);
     const double cos_phi = std::cos(phi);
     const double c =
          semi_major_axis /
          std::sqrt(1.0 - eccentricity_squared * sin_phi * sin_phi);
     const double d = c + g.h;
     p.x = d * cos_phi * std::cos(lambda);
     p.y = d * cos_phi * std::sin(lambda);
     p.z = (one_minus_eccentricity_squared * c + g.h) * sin_phi;
}

double distance(const Cart_coord& p, const Cart_coord& q) {
     return std::hypot(p.x - q.x, p.y - q.y, p.z - q.z);
}

#ifdef HAVE_PUGIXML

void GPX_data::parse(std::istream& f) {
     using std::runtime_error;

     state_ = State::error;

     pugi::xml_document doc;
     pugi::xml_parse_result const result = doc.load(f);
     if (!result)
          throw runtime_error(result.description());
     parse(doc);
}

void GPX_data::parse(char const* fname) {
     using std::runtime_error;

     state_ = State::error;

     pugi::xml_document doc;
     pugi::xml_parse_result const result = doc.load_file(fname);
     if (!result)
          throw runtime_error(result.description());
     parse(doc);
}

double GPX_data::distance() const {
     check_state();
     return distance_;
}

double GPX_data::distance_on_ellipsoid() const {
     check_state();
     return distance_on_ellipsoid_;
}

double GPX_data::uphill() const {
     check_state();
     return uphill_;
}

double GPX_data::downhill() const {
     check_state();
     return downhill_;
}

std::string GPX_data::start_time() const {
     check_state();
     return boost::posix_time::to_iso_extended_string(start_time_);
}

std::string GPX_data::end_time() const {
     check_state();
     return boost::posix_time::to_iso_extended_string(end_time_);
}

std::string GPX_data::elapsed_time() const {
     check_state();
     boost::posix_time::time_duration td = end_time_ - start_time_;
     return boost::posix_time::to_simple_string(td);
}

boost::int64_t GPX_data::elapsed_seconds() const {
     check_state();
     boost::posix_time::time_duration td = end_time_ - start_time_;
     return td.total_seconds();
}

double GPX_data::speedms() const {
     check_state();
     return distance_ / (end_time_ - start_time_).total_seconds();
}

double GPX_data::speedkmh() const {
     return speedms() * 3.6;
}

void GPX_data::parse(pugi::xml_document const& doc) {
     using std::runtime_error;
     using boost::posix_time::ptime;
     using boost::posix_time::from_iso_extended_string;

     distance_ = 0.0;
     distance_on_ellipsoid_ = 0.0;
     uphill_ = 0.0;
     downhill_ = 0.0;
     bool first = true;
     Geogr_coord gcp;   // previous
     Geogr_coord gcc;   // current
     Geogr_coord gcep;  // previous on ellipsoid
     Geogr_coord gcec;  // current on ellipsoid
     Cart_coord ccp;    // previous
     Cart_coord ccc;    // current
     Cart_coord ccep;   // previous on ellipsoid
     Cart_coord ccec;   // current on ellipsoid
     char* str_end;
     char tmp[20];
     boost::posix_time::ptime ctime;
     unsigned ntrkpts = 0;  // number of track points

     gcp = {0.0, 0.0, 0.0};  // to shut up warnings
     gcec.h = 0.0;
     pugi::xml_node ns =
          doc.child("gpx").child("trk").child("trkseg");
     for (pugi::xml_node p = ns.first_child(); p;
          p = p.next_sibling()) {
          char const* v = p.attribute("lat").value();
          if (!*v)
               throw runtime_error("missing latitude");
          gcec.phi = gcc.phi = std::strtod(v, &str_end);
          if (*str_end != '\0' || !std::isfinite(gcc.phi))
               throw runtime_error("invalid latitude");
          if (gcc.phi < -90.0 || gcc.phi > 90.0)
               throw runtime_error("latitude out of range");

          v = p.attribute("lon").value();
          if (!*v)
               throw runtime_error("missing longitude");
          gcec.lambda = gcc.lambda = std::strtod(v, &str_end);
          if (*str_end != '\0' || !std::isfinite(gcc.lambda))
               throw runtime_error("invalid longitude");
          if (gcc.lambda < -180.0 || gcc.lambda >= 180.0)
               throw runtime_error("longitude out of range");

          v = p.child_value("ele");
          if (!*v)
               throw runtime_error("missing elevation");
          gcc.h = std::strtod(v, &str_end);
          if (*str_end != '\0' || !std::isfinite(gcc.h))
               throw runtime_error("invalid elevation");

          // "2021-06-11T16:01:53Z" --> "2021-06-11T16:01:53"
          strncpy(tmp, p.child_value("time"), 19);
          tmp[19] = '\0';
          if (!*tmp)
               throw runtime_error("missing timestamp");
          try {
               ctime = ptime(from_iso_extended_string(tmp));
          } catch (std::exception const&) {
               throw runtime_error("invalid timestamp");
          }

          convert(gcc, ccc);
          convert(gcec, ccec);

          if (first) {
               start_time_ = end_time_ = ctime;
               first = false;
          } else {
               distance_ += SHG::GPS::distance(ccp, ccc);
               distance_on_ellipsoid_ +=
                    SHG::GPS::distance(ccep, ccec);
               if (gcc.h > gcp.h)
                    uphill_ += gcc.h - gcp.h;
               else if (gcc.h < gcp.h)
                    downhill_ += gcp.h - gcc.h;
               if (ctime < end_time_)
                    throw runtime_error("timestamp mismatch");
               end_time_ = ctime;
          }
          gcp = gcc;
          ccp = ccc;
          gcep = gcec;
          ccep = ccec;
          ntrkpts++;
     }
     ns = ns.next_sibling();
     if (ns)
          throw runtime_error("more then one track segment found");
     if (ntrkpts <= 1)
          throw runtime_error("not enough track points");
     if ((end_time_ - start_time_).total_seconds() < 1)
          throw runtime_error("elapsed time is zero seconds");
     state_ = State::ok;
}

void GPX_data::check_state() const {
     if (state_ != State::ok)
          throw std::logic_error(
               "successfully call the method parse() first");
}

void Activity_statistics::run(std::filesystem::path const& dir) {
     namespace fs = std::filesystem;
     namespace io = boost::iostreams;
     using std::ios_base;
     GPX_data d;

     for (auto const& p : fs::directory_iterator(dir)) {
          if (!fs::is_regular_file(p.path()))
               continue;
          io::filtering_streambuf<io::input> inbuf;

          if (p.path().extension() == ".gz" &&
              p.path().stem().extension() == ".gpx") {
               inbuf.push(io::gzip_decompressor());
          } else if (p.path().extension() != ".gpx") {
               continue;
          }
          Result r;
          r.fname = p.path().filename().generic_string();
          r.status = "failed";

          try {
               std::ifstream f(p.path(),
                               ios_base::in | ios_base::binary);
               inbuf.push(f);
               std::istream instream(&inbuf);
               // When not using buf and calling
               // d.parse(instream), on Windows an exception "no
               // random access: iostream stream error" is
               // thrown.
               std::stringstream buf(ios_base::in | ios_base::out |
                                     ios_base::binary);
               buf << instream.rdbuf();
               d.parse(buf);
               if (d.state() == GPX_data::State::ok) {
                    r.status = "ok";
                    r.distance = d.distance();
                    r.distance_on_ellipsoid =
                         d.distance_on_ellipsoid();
                    r.uphill = d.uphill();
                    r.downhill = d.downhill();
                    r.start_time = d.start_time();
                    r.end_time = d.end_time();
                    r.elapsed_time = d.elapsed_time();
                    r.elapsed_seconds = d.elapsed_seconds();
                    r.speedms = d.speedms();
                    r.speedkmh = d.speedkmh();
               }
          } catch (std::exception const& e) {
               r.status += ": ";
               r.status += e.what();
          }
          results_.push_back(r);
     }
}

#endif

}  // namespace SHG::GPS


/**
 * @file q_learning_test.hpp
 * @author Shangtong Zhang
 * @author Rohan Raj
 *
 * Test for Q-Learning implementation
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/reinforcement_learning/q_learning.hpp>
#include <mlpack/methods/reinforcement_learning/environment/mountain_car.hpp>
#include <mlpack/methods/reinforcement_learning/environment/acrobot.hpp>
#include <mlpack/methods/reinforcement_learning/environment/cart_pole.hpp>
#include <mlpack/methods/reinforcement_learning/environment/double_pole_cart.hpp>
#include <mlpack/methods/reinforcement_learning/policy/greedy_policy.hpp>
#include <mlpack/methods/reinforcement_learning/training_config.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace ens;
using namespace mlpack::rl;

BOOST_AUTO_TEST_SUITE(QLearningTest);

//! Test DQN in Cart Pole task.
BOOST_AUTO_TEST_CASE(CartPoleWithDQN)
{
  // Set up the network.
  FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
      GaussianInitialization(0, 0.001));
  model.Add<Linear<>>(4, 128);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(128, 128);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(128, 2);

  // Set up the policy and replay method.
  GreedyPolicy<CartPole> policy(1.0, 1000, 0.1, 0.99);
  RandomReplay<CartPole> replayMethod(10, 10000);

  TrainingConfig config;
  config.StepSize() = 0.01;
  config.Discount() = 0.9;
  config.TargetNetworkSyncInterval() = 100;
  config.ExplorationSteps() = 100;
  config.DoubleQLearning() = false;
  config.StepLimit() = 200;

  // Set up DQN agent.
  QLearning<CartPole, decltype(model), AdamUpdate, decltype(policy)>
      agent(std::move(config), std::move(model), std::move(policy),
      std::move(replayMethod));

  arma::running_stat<double> averageReturn;
  size_t episodes = 0;
  bool converged = true;
  while (true)
  {
    double episodeReturn = agent.Episode();
    averageReturn(episodeReturn);
    episodes += 1;

    if (episodes > 1000)
    {
      Log::Debug << "Cart Pole with DQN failed." << std::endl;
      converged = false;
      break;
    }

    /**
     * Reaching running average return 35 is enough to show it works.
     * For the speed of the test case, I didn't set high criterion.
     */
    Log::Debug << "Average return: " << averageReturn.mean()
        << " Episode return: " << episodeReturn << std::endl;
    if (averageReturn.mean() > 35)
    {
      agent.Deterministic() = true;
      arma::running_stat<double> testReturn;
      for (size_t i = 0; i < 10; ++i)
        testReturn(agent.Episode());

      Log::Debug << "Average return in deterministic test: "
          << testReturn.mean() << std::endl;
      break;
    }
  }
  BOOST_REQUIRE(converged);
}

//! Test DQN in Cart Pole task with Prioritized Replay.
BOOST_AUTO_TEST_CASE(CartPoleWithDQNPrioritizedReplay)
  {
  // Set up the network.
  FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
      GaussianInitialization(0, 0.001));
  model.Add<Linear<>>(4, 128);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(128, 128);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(128, 2);

  // Set up the policy and replay method.
  GreedyPolicy<CartPole> policy(1.0, 1000, 0.1);
  PrioritizedReplay<CartPole> replayMethod(10, 10000, 0.6);

  TrainingConfig config;
  config.StepSize() = 0.01;
  config.Discount() = 0.9;
  config.TargetNetworkSyncInterval() = 100;
  config.ExplorationSteps() = 100;
  config.DoubleQLearning() = false;
  config.StepLimit() = 200;

  // Set up DQN agent.
  QLearning<CartPole, decltype(model), AdamUpdate, decltype(policy),
      decltype(replayMethod)>
      agent(std::move(config), std::move(model), std::move(policy),
          std::move(replayMethod));

  arma::running_stat<double> averageReturn;
  size_t episodes = 0;
  bool converged = true;
  while (true)
  {
    double episodeReturn = agent.Episode();
    averageReturn(episodeReturn);
    episodes += 1;

    if (episodes > 1000)
    {
      Log::Debug << "Cart Pole with DQN failed." << std::endl;
      converged = false;
      break;
    }

    /**
     * Reaching running average return 35 is enough to show it works.
     * For the speed of the test case, I didn't set high criterion.
     */
    Log::Debug << "Average return: " << averageReturn.mean()
        << " Episode return: " << episodeReturn << std::endl;
    if (averageReturn.mean() > 35)
    {
      agent.Deterministic() = true;
      arma::running_stat<double> testReturn;
      for (size_t i = 0; i < 10; ++i)
        testReturn(agent.Episode());

      Log::Debug << "Average return in deterministic test: "
          << testReturn.mean() << std::endl;
      break;
    }
  }

  BOOST_REQUIRE(converged);
}

//! Test Double DQN in Cart Pole task.
BOOST_AUTO_TEST_CASE(CartPoleWithDoubleDQN)
{
  // It isn't guaranteed that the network will converge in the specified number
  // of iterations using random weights. If this works 1 of 4 times, I'm fine
  // with that.
  size_t episodes = 0;
  bool converged = false;
  for (size_t trial = 0; trial < 4; ++trial)
  {
    // Set up the network.
    FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
        GaussianInitialization(0, 0.001));
    model.Add<Linear<>>(4, 20);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(20, 20);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(20, 2);

    // Set up the policy and replay method.
    GreedyPolicy<CartPole> policy(1.0, 1000, 0.1, 0.99);
    RandomReplay<CartPole> replayMethod(10, 10000);

    TrainingConfig config;
    config.StepSize() = 0.01;
    config.Discount() = 0.9;
    config.TargetNetworkSyncInterval() = 100;
    config.ExplorationSteps() = 100;
    config.DoubleQLearning() = false;
    config.StepLimit() = 200;

    // Set up the DQN agent.
    QLearning<CartPole, decltype(model), RMSPropUpdate, decltype(policy)>
        agent(std::move(config), std::move(model), std::move(policy),
        std::move(replayMethod));

    arma::running_stat<double> averageReturn;

    for (episodes = 0; episodes <= 1000; ++episodes)
    {
      double episodeReturn = agent.Episode();
      averageReturn(episodeReturn);

      /**
       * Reaching running average return 35 is enough to show it works.
       * For the speed of the test case, I didn't set high criterion.
       */
      Log::Debug << "Average return: " << averageReturn.mean()
          << " Episode return: " << episodeReturn << std::endl;
      if (averageReturn.mean() > 40)
      {
        agent.Deterministic() = true;
        arma::running_stat<double> testReturn;
        for (size_t i = 0; i < 10; ++i)
          testReturn(agent.Episode());
        Log::Debug << "Average return in deterministic test: "
            << testReturn.mean() << std::endl;
        break;
      }
    }

    if (episodes < 1000)
    {
      converged = true;
      break;
    }
  }

  BOOST_REQUIRE(converged);
}

//! Test DQN in Acrobot task.
BOOST_AUTO_TEST_CASE(AcrobotWithDQN)
{
  // We will allow three trials, although it would be very uncommon for the test
  // to use more than one.
  bool success = false;
  for (size_t trial = 0; trial < 3; ++trial)
  {
    // Set up the network.
    FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
        GaussianInitialization(0, 0.001));
    model.Add<Linear<>>(4, 64);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(64, 32);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(32, 3);

    // Set up the policy and replay method.
    GreedyPolicy<Acrobot> policy(1.0, 1000, 0.1, 0.99);
    RandomReplay<Acrobot> replayMethod(20, 10000);

    TrainingConfig config;
    config.StepSize() = 0.01;
    config.Discount() = 0.99;
    config.TargetNetworkSyncInterval() = 100;
    config.ExplorationSteps() = 100;
    config.DoubleQLearning() = false;
    config.StepLimit() = 400;

    // Set up DQN agent.
    QLearning<Acrobot, decltype(model), AdamUpdate, decltype(policy)>
        agent(std::move(config), std::move(model), std::move(policy),
        std::move(replayMethod));

    arma::running_stat<double> averageReturn;
    size_t episodes = 0;
    bool converged = true;
    while (true)
    {
      double episodeReturn = agent.Episode();
      averageReturn(episodeReturn);
      episodes += 1;

      if (episodes > 1000)
      {
        Log::Debug << "Acrobot with DQN failed." << std::endl;
        converged = false;
        break;
      }

      /**
       * I am using a threshold of -380 to check convergence.
       */
      Log::Debug << "Average return: " << averageReturn.mean()
          << " Episode return: " << episodeReturn << std::endl;
      if (averageReturn.mean() > -380.00)
      {
        agent.Deterministic() = true;
        arma::running_stat<double> testReturn;
        for (size_t i = 0; i < 20; ++i)
          testReturn(agent.Episode());

        Log::Debug << "Average return in deterministic test: "
            << testReturn.mean() << std::endl;
        break;
      }
    }

    if (converged)
    {
      success = true;
      break;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

//! Test DQN in Mountain Car task.
BOOST_AUTO_TEST_CASE(MountainCarWithDQN)
{
  // We will allow three trials total.
  bool success = false;
  for (size_t trial = 0; trial < 3; trial++)
  {
    // Set up the network.
    FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
        GaussianInitialization(0, 0.001));
    model.Add<Linear<>>(2, 64);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(64, 32);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(32, 3);

    // Set up the policy and replay method.
    GreedyPolicy<MountainCar> policy(1.0, 1000, 0.1, 0.99);
    RandomReplay<MountainCar> replayMethod(20, 10000);

    TrainingConfig config;
    config.StepSize() = 0.0001;
    config.Discount() = 0.9;
    config.TargetNetworkSyncInterval() = 100;
    config.ExplorationSteps() = 100;
    config.DoubleQLearning() = false;
    config.StepLimit() = 400;

    // Set up DQN agent.
    QLearning<MountainCar, decltype(model), AdamUpdate, decltype(policy)>
        agent(std::move(config), std::move(model), std::move(policy),
        std::move(replayMethod));

    arma::running_stat<double> averageReturn;
    size_t episodes = 0;
    bool converged = true;
    while (true)
    {
      double episodeReturn = agent.Episode();
      averageReturn(episodeReturn);
      episodes += 1;

      if (episodes > 1000)
      {
        Log::Debug << "Mountain Car with DQN failed." << std::endl;
        converged = false;
        break;
      }

      /**
       * Set a threshold of -370 to check convergence.
       */
      Log::Debug << "Average return: " << averageReturn.mean()
          << " Episode return: " << episodeReturn << std::endl;
      if (averageReturn.mean() > -370)
      {
        agent.Deterministic() = true;
        arma::running_stat<double> testReturn;
        for (size_t i = 0; i < 10; ++i)
          testReturn(agent.Episode());

        Log::Debug << "Average return in deterministic test: "
            << testReturn.mean() << std::endl;
        break;
      }
    }

    if (converged)
    {
      success = true;
      break;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

//! Test DQN in DoublePoleCart task.
BOOST_AUTO_TEST_CASE(DoublePoleCartWithDQN)
{
  // We will allow three trials total.
  bool success = false;
  for (size_t trial = 0; trial < 3; trial++)
  {
    // Set up the network.
    FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
        GaussianInitialization(0, 0.001));
    model.Add<Linear<>>(6, 128);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(128, 32);
    model.Add<ReLULayer<>>();
    model.Add<Linear<>>(32, 3);

    // Set up the policy and replay method.
    GreedyPolicy<DoublePoleCart> policy(1.0, 1000, 0.1, 0.99);
    RandomReplay<DoublePoleCart> replayMethod(20, 10000);

    TrainingConfig config;
    config.StepSize() = 0.001;
    config.Discount() = 0.9;
    config.TargetNetworkSyncInterval() = 100;
    config.ExplorationSteps() = 100;
    config.DoubleQLearning() = false;
    config.StepLimit() = 600;

    // Set up DQN agent.
    QLearning<DoublePoleCart, decltype(model), AdamUpdate, decltype(policy)>
        agent(std::move(config), std::move(model), std::move(policy),
        std::move(replayMethod));

    size_t episodes = 0;
    bool converged = true;
    size_t successive = 0;
    while (true)
    {
      double episodeReturn = agent.Episode();
      episodes += 1;

      if (episodes > 1000)
      {
        Log::Debug << "Multiple Pole Cart with DQN failed." << std::endl;
        converged = false;
        break;
      }

      if (episodeReturn >= 380)
        successive++;
      else if (successive)
        successive = 0;

      // If the model can solve the environment in two consecutive trials this
      // is fine for a simple test.
      if (successive >= 2)
      {
        Log::Debug << "QLearning has succeeded in the multiple pole cart" <<
            " environment." << std::endl;
        converged = true;
        break;
      }
    }

    if (converged)
    {
      success = true;
      break;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

BOOST_AUTO_TEST_SUITE_END();