#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 */


/**
 * \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


#pragma once

#include <array>
#include <random>

#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

namespace affx {

class Affine {
  using Vector6d = Eigen::Matrix<double, 6, 1>;
  using Vector7d = Eigen::Matrix<double, 7, 1>;

  using Euler = Eigen::EulerAngles<double, Eigen::EulerSystemZYX>;
  typedef Eigen::Affine3d Type;

public:
  Type data;

  explicit Affine() {
    this->data = Type::Identity();
  }

  explicit Affine(const Type& data) {
    this->data = data;
  }

  explicit Affine(double x, double y, double z, double a = 0.0, double b = 0.0, double c = 0.0) {
    data.translation() = Eigen::Vector3d(x, y, z);
    data.linear() = Euler(a, b, c).toRotationMatrix();
  }

  explicit Affine(double x, double y, double z, double q_w, double q_x, double q_y, double q_z) {
    data.translation() = Eigen::Vector3d(x, y, z);
    data.linear() = Eigen::Quaterniond(q_w, q_x, q_y, q_z).toRotationMatrix();
  }

  explicit Affine(const std::array<double, 6>& v): Affine(v[0], v[1], v[2], v[3], v[4], v[5]) { }

  explicit Affine(const std::array<double, 7>& v): Affine(v[0], v[1], v[2], v[3], v[4], v[5]) { }

  explicit Affine(const Vector6d& v): Affine(v[0], v[1], v[2], v[3], v[4], v[5]) { }

  explicit Affine(const Vector7d& v): Affine(v[0], v[1], v[2], v[3], v[4], v[5]) { }

  explicit Affine(const std::array<double, 16>& array) {
    Type affine(Eigen::Matrix4d::Map(array.data()));
    data = affine;
  }

  Affine operator *(const Affine &a) const {
    Type result;
    result = data * a.data;
    return Affine(result);
  }

  Affine inverse() const {
    return Affine(data.inverse());
  }

  bool isApprox(const Affine &a) const {
    return data.isApprox(a.data);
  }

  Eigen::Ref<Affine::Type::MatrixType> matrix() {
    return data.matrix();
  }

  std::array<double, 16> array() const {
    std::array<double, 16> array;
    std::copy(data.data(), data.data() + array.size(), array.begin());
    return array;
  }

  Vector6d vector() const {
    Vector6d result;
    result << data.translation(), angles();
    return result;
  }

  Vector7d vector_with_elbow(double elbow) const {
    Vector7d result;
    result << data.translation(), angles(), elbow;
    return result;
  }

  Eigen::Vector3d translation() const {
    Eigen::Vector3d v;
    v << data.translation();
    return v;
  }

  Eigen::Vector3d angles() const {
    Eigen::Vector3d angles = Euler::FromRotation<false, false, false>(data.rotation()).angles();
    Eigen::Vector3d angles_equal;
    angles_equal << angles[0] - M_PI, M_PI - angles[1], angles[2] - M_PI;

    if (angles_equal[1] > M_PI) {
      angles_equal[1] -= 2 * M_PI;
    }
    if (angles_equal[2] < -M_PI) {
      angles_equal[2] += 2 * M_PI;
    }

    if (angles.norm() < angles_equal.norm()) {
      return angles;
    }
    return angles_equal;
  }

  Type::LinearMatrixType rotation() const {
    Type::LinearMatrixType result;
    result << data.rotation();
    return result;
  }

  Eigen::Quaterniond quaternion() const {
    Eigen::Quaterniond q(data.rotation());
    return q;
  }

  std::array<double, 4> py_quaternion() const {
    auto q = quaternion();
    return {q.w(), q.x(), q.y(), q.z()};
  }

  double x() const {
    return data.translation().x();
  }

  double y() const {
    return data.translation().y();
  }

  double z() const {
    return data.translation().z();
  }

  double a() const {
    return angles()(0);
  }

  double b() const {
    return angles()(1);
  }

  double c() const {
    return angles()(2);
  }

  double qW() const {
    return quaternion().w();
  }

  double qX() const {
    return quaternion().x();
  }

  double qY() const {
    return quaternion().y();
  }

  double qZ() const {
    return quaternion().z();
  }

  void translate(const Eigen::Vector3d &v) {
    data.translate(v);
  }

  void pretranslate(const Eigen::Vector3d &v) {
    data.pretranslate(v);
  }

  void rotate(const Type::LinearMatrixType &r) {
    data.rotate(r);
  }

  void prerotate(const Type::LinearMatrixType &r) {
    data.prerotate(r);
  }

  void setQuaternion(double w, double x, double y, double z) {
    data.linear() = Eigen::Quaterniond(w, x, y, z).toRotationMatrix();
  }

  void setX(double x) {
    data.translation().x() = x;
  }

  void setY(double y) {
    data.translation().y() = y;
  }

  void setZ(double z) {
    data.translation().z() = z;
  }

  void setA(double a) {
    Eigen::Vector3d euler = angles();
    data.linear() = Euler(a, euler(1), euler(2)).toRotationMatrix();
  }

  void setB(double b) {
    Eigen::Vector3d euler = angles();
    data.linear() = Euler(euler(0), b, euler(2)).toRotationMatrix();
  }

  void setC(double c) {
    Eigen::Vector3d euler = angles();
    data.linear() = Euler(euler(0), euler(1), c).toRotationMatrix();
  }

  Affine slerp(const Affine& affine, double t) const {
    Type result;
    Eigen::Quaterniond q_start(data.rotation());
    Eigen::Quaterniond q_end(affine.rotation());
    result.translation() = data.translation() + t * (affine.translation() - data.translation());
    result.linear() = q_start.slerp(t, q_end).toRotationMatrix();
    return Affine(result);
  }

  Affine getInnerRandom() const {
    std::random_device r;
    std::default_random_engine engine(r());

    Eigen::Matrix<double, 6, 1> max, random;
    max << data.translation(), angles();

    for (int i = 0; i < 6; i++) {
      std::uniform_real_distribution<double> distribution(-max(i), max(i));
      random(i) = distribution(engine);
    }

    return Affine(random(0), random(1), random(2), random(3), random(4), random(5));
  }

  std::string toString() const {
    Eigen::Matrix<double, 6, 1> v;
    v << data.translation(), angles();

    return "[" + std::to_string(v(0)) + ", " + std::to_string(v(1)) + ", " + std::to_string(v(2))
      + ", " + std::to_string(v(3)) + ", " + std::to_string(v(4)) + ", " + std::to_string(v(5)) + "]";
  }
};

} // namespace affx


// -----------------------------------------------------------------------
//
// Copyright (C) 2020  - David Fernández Castellanos
//
// This file is part of the MEPLS software. You can use it, redistribute
// it, and/or modify it under the terms of the Creative Commons Attribution
// 4.0 International Public License. The full text of the license can be
// found in the file LICENSE at the top level of the MEPLS distribution.
//
// -----------------------------------------------------------------------

#include <deal.II/base/logstream.h>
#include <deal.II/base/path_search.h>

#include "espci.h"

#if defined(OPENMP)
#include <omp.h>

#endif

#include <cmdparser.hpp>
#include <random>

namespace espci
{

using namespace mepls;

void run(const parameters::Parameters &p, dealii::ConditionalOStream & cout)
{
	// TODO here, dim=2 at least for how we set the average pressure

	/////////////////////////////
	//////  setup system ///////
	///////////////////////////

	constexpr unsigned int dim = 2;

	utils::ContinueSimulation continue_simulation;

	auto timer = utils::TimerSingleton::getInstance();
	timer->enter_subsection("Setting up");

	std::mt19937 generator(p.sim.seed);

	std::vector<espci::element::Anisotropic<dim> *> elements_espci = create_elements<dim>(p,
																						  generator);

	mepls::element::Vector<dim> elements;
	for(auto &element : elements_espci)
		elements.push_back(element);

	snapshot::Check<dim> snapshot_check(p.out.snapshots_min, p.out.snapshots_max,
										p.out.snapshots_interval, p.out.snapshots_sensitivity);
	std::vector<snapshot::Threshold<dim> > threshold_snapshots;
	std::vector<snapshot::Stress<dim> > stress_snapshots;
	std::vector<snapshot::DefGrad<dim> > def_grad_snapshots;
	std::vector<patches::PatchPropertiesSnapshot<dim> > patch_prop_snapshots;
	std::vector<GlobalPropertiesSnapshot<dim>> global_properties_snapshots;
	auto patch_to_element_map = patches::make_patch_to_element_map(elements, p.sim.N_patch_list,
																   p.sim.Nx, p.sim.Ny);
	bool parent_liquid = p.sim.parent_liquid;
	bool thermal_relaxation = p.sim.thermal_relaxation;
	bool reload = p.sim.reload;
	bool het_elasticity = p.sim.het_elasticity;
	bool precalculate = not het_elasticity; // precalculate stress factors when doing patch tests;
	// only valid for homogeneous elasticity
	bool do_ee = p.sim.do_ee; // relax from oi state to ee when doing patch tests

	std::vector<double> theta_list;
	if(p.sim.n_theta == 4)
	{
		theta_list.push_back(0 * M_PI / 180.);
		theta_list.push_back(90 * M_PI / 180.);
		theta_list.push_back(50 * M_PI / 180.);
		theta_list.push_back(140 * M_PI / 180.);
	}
	else if(p.sim.n_theta == 2)
	{
		theta_list.push_back(0 * M_PI / 180.);
		theta_list.push_back(90 * M_PI / 180.);
	}
	else
	{
		for(double z = 0.; z < M_PI; z += M_PI / double(p.sim.n_theta))
			theta_list.push_back(z);
	}

	elasticity_solver::LeesEdwards<dim> solver(p.sim.Nx, p.sim.Ny);
	M_Assert(elements.size() == solver.get_n_elements(), "Numbers of mesoscale and finite "
													  "elements do not match");

	// initial (default) assembly using the quench elastic properties.
	for(auto &element : elements)
		solver.set_elastic_properties(element->number(), element->C());
	solver.setup_and_assembly();

	if(het_elasticity)
		mepls::element::calculate_local_stress_coefficients(elements, solver);
	else
		mepls::element::calculate_local_stress_coefficients_central(elements, solver);

	mepls::element::calculate_ext_stress_coefficients(elements, solver);

	system::Standard<dim> system(elements, solver, generator);
	system::MacroState<dim> &macrostate = system.macrostate;

	if(p.mat.init_eigenstrain)
		apply_initial_eigenstrain<dim>(system, p);

	timer->leave_subsection("Setting up");

	///////////////////////////////////////////
	///////  simulate parent liquid //////////
	/////////////////////////////////////////

	timer->enter_subsection("Simulate parent liquid");

	history::History<dim> liquid_history("parent_liquid");
	system.set_history(liquid_history);
	if(parent_liquid)
	{

		for(auto &element : elements_espci)
		{
			auto conf = element->config();
			conf.temperature = p.mat.temperature_liquid;
			element->config(conf);
		}

		simulate_parent_liquid_KMC(system, liquid_history, p, continue_simulation);
//		simulate_parent_liquid_MH(system, liquid_history, p, continue_simulation);
	}

	// apply instantaneous quench
	for(auto &element : elements_espci)
	{
		auto conf = element->config();
		conf.temperature = 0;
		element->config(conf);
	}

	dynamics::relaxation(system, continue_simulation, 1e10);
	if(not continue_simulation())
	{
		cout << continue_simulation << std::endl;
		abort();
	}

	liquid_history.add_macro(system);
	system.macrostate.clear();

	timer->leave_subsection("Simulate parent liquid");

	////////////////////////////
	//////      AQS     ///////
	//////////////////////////

	history::History<dim> aqs_history("AQS");
	system.set_history(aqs_history);

	/* ----- snapshots ----- */
	timer->enter_subsection("Taking snapshots");

	if(p.out.snapshots.find("slip_thresholds") != std::string::npos)
		threshold_snapshots.push_back(
			snapshot::Threshold(system, p.sim.monitor_name, 0.,
								macrostate[p.sim.monitor_name]));
	if(p.out.snapshots.find("stress") != std::string::npos)
		stress_snapshots.push_back(
			snapshot::Stress(system, p.sim.monitor_name, 0.,
							 macrostate[p.sim.monitor_name]));
	if(p.out.snapshots.find("def_grad") != std::string::npos)
		def_grad_snapshots.push_back(
			snapshot::DefGrad(system, p.sim.monitor_name, 0.,
							  macrostate[p.sim.monitor_name]));

	if(p.out.snapshots.find("patches") != std::string::npos)
		for(auto n_patch : p.sim.N_patch_list)
		{
			cout << ">>> " << n_patch << std::endl;
			patch_prop_snapshots.push_back(
				patches::PatchPropertiesSnapshot<dim>(system,
													  p.sim.monitor_name, 0.,
													  macrostate[p.sim.monitor_name],
													  n_patch, theta_list, precalculate,
													  do_ee));
		}

	if(p.out.snapshots.find("global_properties") != std::string::npos)
		global_properties_snapshots.push_back(
			GlobalPropertiesSnapshot<dim>(system, solver, aqs_history.index(), p.sim.monitor_name,
										  0., macrostate[p.sim.monitor_name]));
	timer->leave_subsection("Taking snapshots");

	/* ----- save struct. properties ----- */
	std::vector<event::RenewSlip<dim>> renewal_vector;
	for(auto &element : elements)
		element->record_structural_properties(renewal_vector);
	aqs_history.add(renewal_vector);

	/* ---- simulation loop ----- */
	timer->enter_subsection("Running AQS");

	continue_simulation(system.macrostate[p.sim.monitor_name] < p.sim.monitor_limit,
						p.sim.monitor_name + " limit reached");

	// adding an (empty) driving event will record the average prestress
	// (which can be slightly non-zero due to numerical accuray) as the
	// first event in the history
	event::Driving<dim> prestress_event;
	prestress_event.activation_protocol = mepls::dynamics::Protocol::prestress;
	system.add(prestress_event);

	aqs_history.add_macro(system);

	assert( macrostate["av_vm_plastic_strain"]==0. );

	double G_old = 0.;
	double G_stat = p.mat.G;
	while(continue_simulation())
	{
		// reassemble the elastic properties if the change in the shear
		// modulus is big enough and if it's different enough from the stationary
		// value
		dealii::SymmetricTensor<4,dim> av_C;
		for(auto &element : elements)
			av_C += element->C();
		av_C /= double(elements.size());

		double G_new = av_C[0][1][0][1];

		if(std::abs(G_new/G_old-1)>0.001 and std::abs(G_old/G_stat-1)>0.005)
		{
			timer->leave_subsection("Running AQS");
			timer->enter_subsection("Reassembling with new stiffness");

			G_old = G_new;

			solver.reassemble_with_new_stiffness(av_C);

			// we call add which will inform the macrostate and the elements
			// about the change in external stress due to the change in global
			// stiffness and will also record that change in the history
			const std::vector<event::Plastic<dim>> added_yielding;
			event::Driving<dim> driving_event_variation_stiffness;
			driving_event_variation_stiffness.activation_protocol = mepls::dynamics::Protocol::variation_stiffness;
			system.add(driving_event_variation_stiffness);

			auto state = solver.get_state();
			mepls::element::calculate_local_stress_coefficients_central(elements, solver);
			mepls::element::calculate_ext_stress_coefficients(elements, solver);
			solver.set_state(state);

			// relax to ensure there are no unstable elements after the change
			// in the elastic properties (if the shear modulus rises with strain,
			// that will lead to stress rises that can unstabilise elements)
			dynamics::relaxation(system, continue_simulation);

			timer->leave_subsection("Reassembling with new stiffness");
			timer->enter_subsection("Running AQS");
		}

		cout << aqs_history.index() << " | " << std::fixed << macrostate["total_strain"]
				  << " " << macrostate["ext_stress"] << std::endl;

		if(snapshot_check(macrostate[p.sim.monitor_name]))
		{
			timer->leave_subsection("Running AQS");
			timer->enter_subsection("Taking snapshots");

			if(p.out.snapshots.find("slip_thresholds") != std::string::npos)
				threshold_snapshots.push_back(
					snapshot::Threshold(system, p.sim.monitor_name,
										snapshot_check.desired_value,
										macrostate[p.sim.monitor_name]));
			if(p.out.snapshots.find("stress") != std::string::npos)
				stress_snapshots.push_back(
					snapshot::Stress(system, p.sim.monitor_name,
									 snapshot_check.desired_value, macrostate[p.sim.monitor_name]));
			if(p.out.snapshots.find("def_grad") != std::string::npos)
				def_grad_snapshots.push_back(
					snapshot::DefGrad(system, p.sim.monitor_name,
									  snapshot_check.desired_value,
									  macrostate[p.sim.monitor_name]));
			if(p.out.snapshots.find("patches") != std::string::npos)
				for(auto n_patch : p.sim.N_patch_list)
				{
					cout << ">>> " << n_patch << std::endl;
					patch_prop_snapshots.push_back(
						patches::PatchPropertiesSnapshot<dim>(system,
															  p.sim.monitor_name,
															  snapshot_check.desired_value,
															  macrostate[p.sim.monitor_name],
															  n_patch,
															  theta_list, precalculate, do_ee));
				}

			timer->leave_subsection("Taking snapshots");
			timer->enter_subsection("Running AQS");
		}

		/* ---- dyanmics ----- */
		dynamics::finite_extremal_dynamics_step(1e-4 * 0.5, system);
		aqs_history.add_macro(system);

		dynamics::relaxation(system, continue_simulation);
		aqs_history.add_macro(system);

		continue_simulation(system.macrostate[p.sim.monitor_name] < p.sim.monitor_limit,
							p.sim.monitor_name + " limit reached");
	}

	auto solver_state_end_AQS = solver.get_state();
	auto macrostate_end_AQS = system.macrostate;

	cout << continue_simulation << std::endl;

	timer->leave_subsection("Running AQS");

	//////////////////////////////////
	//////  thermal relaxation //////
	////////////////////////////////

	timer->enter_subsection("Thermal relaxation");

	history::History<dim> thermal_relaxation_hist("thermal_relaxation");

	if(thermal_relaxation)
	{
		mepls::element::Vector<dim> elements_replica;
		for(auto &element : elements_espci)
		{
			auto element_copy = element->make_copy();

			auto conf = element_copy->config();
			conf.temperature = p.mat.temperature_relaxation;
			element_copy->config(conf);

			elements_replica.push_back( element_copy );
		}

		// the solver state is copied, so it contains the eigenstrain and the load
		// in this case the elements elastic field need not be converted into a
		// prestress before
		solver.set_state(solver_state_end_AQS);

		auto system_replica = system.get_new_instance(elements_replica, solver,
													  system.generator);
		system_replica->macrostate = macrostate_end_AQS;
		system_replica->set_history(thermal_relaxation_hist);

		// initiate dynamics using copied system
		mepls::dynamics::KMC<dim> kmc;
		mepls::utils::ContinueSimulation continue_relaxing;
		auto &macrostate = system_replica->macrostate;
		thermal_relaxation_hist.add_macro( *system_replica );

		while(continue_relaxing())
		{

			cout << thermal_relaxation_hist.index() << " | " << std::fixed
					  << macrostate["total_strain"]
					  << " " << macrostate["ext_stress"]
					  << " " << macrostate["pressure"]
					  << " " << macrostate["time"]
					  << std::endl;

			kmc(*system_replica);
			thermal_relaxation_hist.add_macro( *system_replica );

			mepls::dynamics::relaxation(*system_replica, continue_relaxing);
			thermal_relaxation_hist.add_macro( *system_replica );

			continue_relaxing(macrostate["ext_stress"] > 0, "System relaxed");
		}

		delete system_replica;

		cout << continue_relaxing << std::endl;
		cout << "Relaxing finished" << std::endl;
	}

	timer->leave_subsection("Thermal relaxation");

	///////////////////////////////
	//////  reloading step ///////
	/////////////////////////////

	history::History<dim> aqs_unloading("AQS_unloading");
	history::History<dim> aqs_reload_forward("AQS_reload_forward");
	history::History<dim> aqs_reload_backward("AQS_reload_backward");

	if(reload)
	{
		timer->enter_subsection("Reloading");

		// the solver state is copied, so it contains the eigenstrain and the load
		// in this case the elements elastic field need not be converted into a
		// prestress before
		solver.set_state(solver_state_end_AQS);

		system.macrostate = macrostate_end_AQS;

		system.set_history(aqs_unloading);

		utils::ContinueSimulation continue_unloading;
		auto &macrosate = system.macrostate;
		aqs_unloading.add_macro(system);

		while(continue_unloading())
		{
			cout << aqs_unloading.index() << " | " << std::fixed
					  << macrostate["total_strain"] << " " << macrostate["ext_stress"] << " "
					  << macrostate["pressure"] << std::endl;

			dynamics::finite_extremal_dynamics_step(1e-4 * 0.5, system, false);
			aqs_unloading.add_macro(system);

			dynamics::relaxation(system, continue_unloading);
			aqs_unloading.add_macro(system);

			continue_unloading(macrostate["ext_stress"] > 0, "System unloaded");
		}

		cout << continue_unloading << std::endl;

		perform_reloading(system, aqs_reload_forward, true, p);
		perform_reloading(system, aqs_reload_backward, false, p);

		timer->leave_subsection("Reloading");
	}

	/////////////////////////////////////
	//////  post-simultaion step ///////
	///////////////////////////////////

	std::string filename = p.out.filename + "_" + write::make_filename(p) + ".h5";
	H5::H5File file(p.out.path + "/" + filename, H5F_ACC_TRUNC);
	write::file_attrs(file, p);

	if(parent_liquid)
		write::evolution_history(file, liquid_history);
	if(thermal_relaxation)
		write::evolution_history(file, thermal_relaxation_hist);
	write::evolution_history(file, aqs_history);
	if(reload)
	{
		write::evolution_history(file, aqs_unloading);
		write::evolution_history(file, aqs_reload_forward);
		write::evolution_history(file, aqs_reload_backward);
	}

	write::patch_info<dim>(file, "/patch_info", patch_to_element_map);
	write::element_info(file, "/element_info", elements);
	write::snapshots(file, "/snapshots", threshold_snapshots, stress_snapshots, def_grad_snapshots,
					 patch_prop_snapshots, global_properties_snapshots);
	file.close();

	/* ---- delete dynamically-allocated objects ----- */
	for(auto &element : elements)
		delete element;

	if(p.out.verbosity and omp_get_thread_num() == 0)
		timer->print_summary();
} // run

} // espci

int main(int argc, char *argv[])
{
	cli::Parser parser(argc, argv);
	parser.set_optional<std::string>("f", "file", "./default.cfg",
									 "Name of the input configuration file");
	parser.run_and_exit_if_error();

	dealii::deallog.depth_console(0);

	espci::parameters::Parameters p;

	try
	{
		p.load_file(parser.get<std::string>("f"));
	}
	catch(dealii::PathSearch::ExcFileNotFound &)
	{
		p.generate_file(parser.get<std::string>("f"));
		std::cout << "Configuration file " << parser.get<std::string>("f") << " created" << std::endl;
	}

	unsigned int n_rep = p.sim.n_rep;
	if(n_rep < omp_get_max_threads())
		n_rep = omp_get_max_threads();

	// initialize the master engine with the master seed
	std::srand(p.sim.seed);

	#pragma omp parallel
	{
		unsigned int n_threads = omp_get_max_threads();
		unsigned int id = omp_get_thread_num();
		unsigned int rep_per_thread = int( n_rep / n_threads );

		dealii::ConditionalOStream cout(std::cout, id==0 and p.out.verbosity);

		for(unsigned int n = 0; n < rep_per_thread; ++n)
		{
			espci::parameters::Parameters p_thread = p;
			#pragma critical
			{
				// generate a seed for a simulation run
				p_thread.sim.seed = std::rand();
			};

			espci::run(p_thread, cout);
		}

	}

}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2022, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "poses-precomp.h"	// Precompiled headers
//
#include <mrpt/math/ops_containers.h>  // maximum()
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/poses/CPointPDFParticles.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::system;

IMPLEMENTS_SERIALIZABLE(CPointPDFParticles, CPointPDF, mrpt::poses)

CPointPDFParticles::CPointPDFParticles(size_t numParticles)
{
	setSize(numParticles);
}

/** Clear all the particles (free memory) */
void CPointPDFParticles::clear() { setSize(0); }
/*---------------------------------------------------------------
		setSize
  ---------------------------------------------------------------*/
void CPointPDFParticles::setSize(
	size_t numberParticles, const mrpt::math::TPoint3Df& defaultValue)
{
	// Free old particles: automatic via smart ptr
	m_particles.resize(numberParticles);
	for (auto& it : m_particles)
	{
		it.log_w = 0;
		it.d.reset(new TPoint3Df(defaultValue));
	}
}

/*---------------------------------------------------------------
						getMean
  Returns an estimate of the pose, (the mean, or mathematical expectation of the
 PDF)
 ---------------------------------------------------------------*/
void CPointPDFParticles::getMean(CPoint3D& p) const
{
	MRPT_START
	if (m_particles.empty())
		THROW_EXCEPTION("Cannot compute mean since there are zero particles.");

	CParticleList::const_iterator it;
	double sumW = 0;
	double x = 0, y = 0, z = 0;
	for (it = m_particles.begin(); it != m_particles.end(); it++)
	{
		const double w = exp(it->log_w);
		x += it->d->x * w;
		y += it->d->y * w;
		z += it->d->z * w;
		sumW += w;
	}

	ASSERT_(sumW != 0);

	sumW = 1.0 / sumW;

	p.x(x * sumW);
	p.y(y * sumW);
	p.z(z * sumW);

	MRPT_END
}

std::tuple<CMatrixDouble33, CPoint3D> CPointPDFParticles::getCovarianceAndMean()
	const
{
	MRPT_START

	CPoint3D mean;
	CMatrixDouble33 cov;

	getMean(mean);
	cov.setZero();

	size_t i, n = m_particles.size();
	double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;

	double lin_w_sum = 0;

	for (i = 0; i < n; i++)
		lin_w_sum += exp(m_particles[i].log_w);
	if (lin_w_sum == 0) lin_w_sum = 1;

	for (i = 0; i < n; i++)
	{
		double w = exp(m_particles[i].log_w) / lin_w_sum;

		double err_x = m_particles[i].d->x - mean.x();
		double err_y = m_particles[i].d->y - mean.y();
		double err_phi = m_particles[i].d->z - mean.z();

		var_x += square(err_x) * w;
		var_y += square(err_y) * w;
		var_p += square(err_phi) * w;
		var_xy += err_x * err_y * w;
		var_xp += err_x * err_phi * w;
		var_yp += err_y * err_phi * w;
	}

	if (n >= 2)
	{
		// Unbiased estimation of variance:
		cov(0, 0) = var_x;
		cov(1, 1) = var_y;
		cov(2, 2) = var_p;

		cov(1, 0) = cov(0, 1) = var_xy;
		cov(2, 0) = cov(0, 2) = var_xp;
		cov(1, 2) = cov(2, 1) = var_yp;
	}

	return {cov, mean};
	MRPT_END
}

uint8_t CPointPDFParticles::serializeGetVersion() const { return 0; }
void CPointPDFParticles::serializeTo(mrpt::serialization::CArchive& out) const
{
	uint32_t N = size();
	out << N;

	for (const auto& m_particle : m_particles)
		out << m_particle.log_w << m_particle.d->x << m_particle.d->y
			<< m_particle.d->z;
}

void CPointPDFParticles::serializeFrom(
	mrpt::serialization::CArchive& in, uint8_t version)
{
	switch (version)
	{
		case 0:
		{
			uint32_t N;
			in >> N;
			setSize(N);

			for (auto& m_particle : m_particles)
				in >> m_particle.log_w >> m_particle.d->x >> m_particle.d->y >>
					m_particle.d->z;
		}
		break;
		default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
	};
}

void CPointPDFParticles::copyFrom(const CPointPDF& o)
{
	if (this == &o) return;	 // It may be used sometimes

	// Convert to samples:
	THROW_EXCEPTION("NO");
}

/*---------------------------------------------------------------

  ---------------------------------------------------------------*/
bool CPointPDFParticles::saveToTextFile(const std::string& file) const
{
	MRPT_START

	FILE* f = os::fopen(file.c_str(), "wt");
	if (!f) return false;

	size_t i, N = m_particles.size();
	for (i = 0; i < N; i++)
		os::fprintf(
			f, "%f %f %f %e\n", m_particles[i].d->x, m_particles[i].d->y,
			m_particles[i].d->z, m_particles[i].log_w);

	os::fclose(f);
	return true;
	MRPT_END
}

/*---------------------------------------------------------------
						changeCoordinatesReference
 ---------------------------------------------------------------*/
void CPointPDFParticles::changeCoordinatesReference(
	const CPose3D& newReferenceBase)
{
	TPoint3D pt;
	for (auto& m_particle : m_particles)
	{
		newReferenceBase.composePoint(
			m_particle.d->x, m_particle.d->y, m_particle.d->z,	// In
			pt.x, pt.y, pt.z  // Out
		);
		m_particle.d->x = d2f(pt.x);
		m_particle.d->y = d2f(pt.y);
		m_particle.d->z = d2f(pt.z);
	}
}

/*---------------------------------------------------------------
						computeKurtosis
 ---------------------------------------------------------------*/
double CPointPDFParticles::computeKurtosis()
{
	MRPT_START

	// kurtosis = \mu^4 / (\sigma^2) -3
	Eigen::Vector3d kurts, mu4, m, var;
	kurts.fill(0);
	mu4.fill(0);
	m.fill(0);
	var.fill(0);

	// Means:
	for (auto& m_particle : m_particles)
	{
		m[0] += m_particle.d->x;
		m[1] += m_particle.d->y;
		m[2] += m_particle.d->z;
	}
	m *= 1.0 / m_particles.size();

	// variances:
	for (auto& m_particle : m_particles)
	{
		var[0] += square(m_particle.d->x - m[0]);
		var[1] += square(m_particle.d->y - m[1]);
		var[2] += square(m_particle.d->z - m[2]);
	}
	var *= 1.0 / m_particles.size();
	var[0] = square(var[0]);
	var[1] = square(var[1]);
	var[2] = square(var[2]);

	// Moment:
	for (auto& m_particle : m_particles)
	{
		mu4[0] += pow(m_particle.d->x - m[0], 4.0);
		mu4[1] += pow(m_particle.d->y - m[1], 4.0);
		mu4[2] += pow(m_particle.d->z - m[2], 4.0);
	}
	mu4 *= 1.0 / m_particles.size();

	// Kurtosis's
	kurts.array() = mu4.array() / var.array();

	return math::maximum(kurts);

	MRPT_END
}

/*---------------------------------------------------------------
					drawSingleSample
  ---------------------------------------------------------------*/
void CPointPDFParticles::drawSingleSample([
	[maybe_unused]] CPoint3D& outSample) const
{
	THROW_EXCEPTION("TO DO!");
}

/*---------------------------------------------------------------
					bayesianFusion
 ---------------------------------------------------------------*/
void CPointPDFParticles::bayesianFusion(
	[[maybe_unused]] const CPointPDF& p1_,
	[[maybe_unused]] const CPointPDF& p2_,
	[[maybe_unused]] const double minMahalanobisDistToDrop)
{
	MRPT_START

	THROW_EXCEPTION("TODO!!!");

	MRPT_END
}


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

#include "theia/sfm/transformation/gdls_similarity_transform.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <cmath>
#include <glog/logging.h>
#include <vector>

#include "theia/alignment/alignment.h"
#include "theia/sfm/pose/dls_impl.h"
#include "theia/util/random.h"

namespace theia {

using dls_impl::CreateMacaulayMatrix;
using dls_impl::ExtractJacobianCoefficients;
using dls_impl::LeftMultiplyMatrix;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Matrix4d;
using Eigen::MatrixXd;
using Eigen::Quaterniond;
using Eigen::Vector3d;

// This implementation is based off of the DLS PnP implementation. The general
// approach is to first rewrite the reprojection constraint (i.e., cost
// function) such that all unknowns appear linearly in terms of the rotation
// parameters (which are 3 parameters in the Cayley-Gibss-Rodriguez
// formulation). Then we create a system of equations from the jacobian of the
// cost function, and solve these equations via a Macaulay matrix to obtain the
// roots (i.e., the 3 parameters of rotation). The translation and scale can
// then be obtained through back-substitution.
void GdlsSimilarityTransform(const std::vector<Vector3d>& ray_origin,
                             const std::vector<Vector3d>& ray_direction,
                             const std::vector<Vector3d>& world_point,
                             std::vector<Quaterniond>* solution_rotation,
                             std::vector<Vector3d>* solution_translation,
                             std::vector<double>* solution_scale) {
  CHECK_GE(ray_direction.size(), 4);

  const int num_correspondences = ray_direction.size();
  // The bottom-right symmetric block matrix of inverse(A^T * A). This is the
  // generalized version of Matrix H from Eq. 17 in the Appendix of the gDLS
  // paper (note that term appears exactly in the bottom right 3x3 of this
  // matrix).
  Matrix4d h_inverse = Matrix4d::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    h_inverse(0, 0) = h_inverse(0, 0) + ray_origin[i].squaredNorm() -
                      ray_origin[i].dot(ray_direction[i]) *
                          ray_origin[i].dot(ray_direction[i]);
    Vector3d temp_term =
        -ray_origin[i] + ray_origin[i].dot(ray_direction[i]) * ray_direction[i];
    h_inverse.block<3, 1>(1, 0) = h_inverse.block<3, 1>(1, 0) + temp_term;
    h_inverse.block<1, 3>(0, 1) =
        h_inverse.block<1, 3>(0, 1) + temp_term.transpose();
    h_inverse.block<3, 3>(1, 1) =
        h_inverse.block<3, 3>(1, 1) + Matrix3d::Identity() -
        (ray_direction[i] * ray_direction[i].transpose());
  }
  const Matrix4d h_matrix = h_inverse.inverse();

  // This is the translation and scale parameterized by the 9 entries of the
  // rotation matrix. The first row is the scale and rows 2 - 4 are the
  // translation.
  Matrix<double, 4, 9> sv_helper = Matrix<double, 4, 9>::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    // Scale factor.
    sv_helper.row(0) =
        sv_helper.row(0) +
        (ray_origin[i].transpose() -
         ray_origin[i].dot(ray_direction[i]) * ray_direction[i].transpose()) *
            LeftMultiplyMatrix(world_point[i]);

    // Translation factor.
    sv_helper.block<3, 9>(1, 0) =
        sv_helper.block<3, 9>(1, 0) +
        (ray_direction[i] * ray_direction[i].transpose() -
         Matrix3d::Identity()) *
            LeftMultiplyMatrix(world_point[i]);
  }

  sv_helper = h_matrix * sv_helper;
  const Matrix<double, 1, 9>& scale_factor = sv_helper.row(0);
  const Matrix<double, 3, 9>& translation_factor = sv_helper.block<3, 9>(1, 0);

  // Compute the cost function C' of Eq. 15 in gDLS paper. This is a factorized
  // version where the rotation matrix parameters have been pulled out. The
  // entries to this equation are the coefficients to the cost function which is
  // a quartic in the rotation parameters.
  Matrix<double, 9, 9> ls_cost_coefficients = Matrix<double, 9, 9>::Zero();
  for (int i = 0; i < num_correspondences; i++) {
    const Matrix<double, 3, 9> cost_coeff_term =
        (ray_direction[i] * ray_direction[i].transpose() -
         Matrix3d::Identity()) *
        (LeftMultiplyMatrix(world_point[i]) - ray_origin[i] * scale_factor +
         translation_factor);
    ls_cost_coefficients =
        ls_cost_coefficients + cost_coeff_term.transpose() * cost_coeff_term;
  }

  // Extract the coefficients of the jacobian (Eq. 16) from the
  // ls_cost_coefficients matrix. The jacobian represent 3 monomials in the
  // rotation parameters. Each entry of the jacobian will be 0 at the roots of
  // the polynomial, so we can arrange a system of polynomials from these
  // equations.
  double f1_coeff[20];
  double f2_coeff[20];
  double f3_coeff[20];
  ExtractJacobianCoefficients(
      ls_cost_coefficients, f1_coeff, f2_coeff, f3_coeff);

  // We create one equation with random terms that is generally non-zero at the
  // roots of our system.
  const Eigen::Vector4d rand_vec = 100.0 * Eigen::Vector4d::Random();
  const double macaulay_term[4] = {
      rand_vec(0), rand_vec(1), rand_vec(2), rand_vec(3)};

  // Create Macaulay matrix that will be used to solve our polynonomial system.
  const MatrixXd& macaulay_matrix =
      CreateMacaulayMatrix(f1_coeff, f2_coeff, f3_coeff, macaulay_term);

  // Via the Schur complement trick, the top-left of the Macaulay matrix
  // contains a multiplication matrix whose eigenvectors correspond to solutions
  // to our system of equations.
  const MatrixXd solution_polynomial =
      macaulay_matrix.block<27, 27>(0, 0) -
      (macaulay_matrix.block<27, 93>(0, 27) *
       macaulay_matrix.block<93, 93>(27, 27).partialPivLu().solve(
           macaulay_matrix.block<93, 27>(27, 0)));

  // Extract eigenvectors of the solution polynomial to obtain the roots which
  // are contained in the entries of the eigenvectors.
  const Eigen::EigenSolver<MatrixXd> eigen_solver(solution_polynomial);

  // Many of the eigenvectors will contain complex solutions so we must filter
  // them to find the real solutions.
  const auto eigen_vectors = eigen_solver.eigenvectors();
  for (int i = 0; i < 27; i++) {
    // The first entry of the eigenvector should equal 1 according to our
    // polynomial, so we must divide each solution by the first entry.
    std::complex<double> s1 = eigen_vectors(9, i) / eigen_vectors(0, i);
    std::complex<double> s2 = eigen_vectors(3, i) / eigen_vectors(0, i);
    std::complex<double> s3 = eigen_vectors(1, i) / eigen_vectors(0, i);

    // If the rotation solutions are real, treat this as a valid candidate
    // rotation.
    const double kEpsilon = 1e-6;
    if (fabs(s1.imag()) < kEpsilon && fabs(s2.imag()) < kEpsilon &&
        fabs(s3.imag()) < kEpsilon) {
      // Compute the rotation (which is the transpose rotation of our solution)
      // and translation.
      Quaterniond soln_rotation(1.0, s1.real(), s2.real(), s3.real());
      soln_rotation = soln_rotation.inverse().normalized();

      const Matrix3d rot_mat = soln_rotation.inverse().toRotationMatrix();
      const Eigen::Map<const Matrix<double, 9, 1> > rot_vec(rot_mat.data());
      const Vector3d soln_translation = translation_factor * rot_vec;
      const double soln_scale = scale_factor * rot_vec;

      // TODO(cmsweeney): evaluate cost function and return it as an output
      // variable.

      // Check that all points are in front of the camera. Discard the solution
      // if this is not the case.
      bool all_points_in_front_of_camera = true;

      for (int j = 0; j < num_correspondences; j++) {
        const Vector3d transformed_point = soln_rotation * world_point[j] +
                                           soln_translation -
                                           soln_scale * ray_origin[j];

        // Find the rotation that puts the image ray at [0, 0, 1] i.e. looking
        // straightforward from the camera.
        const Quaterniond unrot =
            Quaterniond::FromTwoVectors(ray_direction[j], Vector3d(0, 0, 1));

        // Rotate the transformed point and check if the z coordinate is
        // negative. This will indicate if the point is projected behind the
        // camera.
        const Vector3d rotated_projection = unrot * transformed_point;
        if (rotated_projection.z() < 0) {
          all_points_in_front_of_camera = false;
          break;
        }
      }

      if (all_points_in_front_of_camera) {
        solution_rotation->push_back(soln_rotation);
        solution_translation->push_back(soln_translation);
        solution_scale->push_back(soln_scale);
      }
    }
  }
}

}  // namespace theia


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HISTOGRAM_ACCUMULATORS_MEAN_HPP
#define BOOST_HISTOGRAM_ACCUMULATORS_MEAN_HPP

#include <boost/histogram/fwd.hpp>
#include <cstddef>
#include <type_traits>

namespace boost {
namespace histogram {
namespace accumulators {

/** Calculates mean and variance of sample.

  Uses Welfords's incremental algorithm to improve the numerical
  stability of mean and variance computation.
*/
template <class RealType>
class mean {
public:
  mean() = default;
  mean(const std::size_t n, const RealType& mean, const RealType& variance)
      : sum_(n), mean_(mean), sum_of_deltas_squared_(variance * (sum_ - 1)) {}

  void operator()(const RealType& x) {
    sum_ += 1;
    const auto delta = x - mean_;
    mean_ += delta / sum_;
    sum_of_deltas_squared_ += delta * (x - mean_);
  }

  template <class T>
  mean& operator+=(const mean<T>& rhs) {
    const auto tmp = mean_ * sum_ + static_cast<RealType>(rhs.mean_ * rhs.sum_);
    sum_ += rhs.sum_;
    mean_ = tmp / sum_;
    sum_of_deltas_squared_ += static_cast<RealType>(rhs.sum_of_deltas_squared_);
    return *this;
  }

  mean& operator*=(const RealType& s) {
    mean_ *= s;
    sum_of_deltas_squared_ *= s * s;
    return *this;
  }

  template <class T>
  bool operator==(const mean<T>& rhs) const noexcept {
    return sum_ == rhs.sum_ && mean_ == rhs.mean_ &&
           sum_of_deltas_squared_ == rhs.sum_of_deltas_squared_;
  }

  template <class T>
  bool operator!=(const mean<T>& rhs) const noexcept {
    return !operator==(rhs);
  }

  std::size_t count() const noexcept { return sum_; }
  const RealType& value() const noexcept { return mean_; }
  RealType variance() const { return sum_of_deltas_squared_ / (sum_ - 1); }

  template <class Archive>
  void serialize(Archive&, unsigned /* version */);

private:
  std::size_t sum_ = 0;
  RealType mean_ = 0, sum_of_deltas_squared_ = 0;
};

} // namespace accumulators
} // namespace histogram
} // namespace boost

#ifndef BOOST_HISTOGRAM_DOXYGEN_INVOKED
namespace std {
template <class T, class U>
/// Specialization for boost::histogram::accumulators::mean.
struct common_type<boost::histogram::accumulators::mean<T>,
                   boost::histogram::accumulators::mean<U>> {
  using type = boost::histogram::accumulators::mean<common_type_t<T, U>>;
};
} // namespace std
#endif

#endif