/**
	@file rompc_utils.cpp
	Utility functions to support the ROMPC controller.
*/

#include <cmath>
#include <Eigen/Geometry>

#include <rompc/rompc_utils.hpp>
#include <utils/rotation.hpp>
#include <utils/data.hpp>

/**
    @brief Constructor, initialize target class
*/
ROMPC_UTILS::Target::Target() {
    _p_r_i_I.setZero();
    _v_r_I_R.setZero();
    _q_I_to_R.setZero();
    _om_R_I_R.setZero();
    _euler.setZero();
    _aa_I_to_R.setZero();
}

/**
    @brief Returns the position of the target w.r.t the
    inertial frame, in inertial frame coordinates
*/
Vec3 ROMPC_UTILS::Target::get_pos(double t) {
    return _p_r_i_I;
}

/**
    @brief Returns the target inertial velocity in
    target frame coordinates
*/
Vec3 ROMPC_UTILS::Target::get_vel(double t) {
    return _v_r_I_R;
}

/**
    @brief Returns attitude quaternion of FRD target
    w.r.t inertial NED frame
*/
Vec4 ROMPC_UTILS::Target::get_att_quat(double t) {
    return _q_I_to_R;
}

/**
    @brief Returns attitude euler angles of FRD target
    w.r.t inertial NED frame
*/
Vec3 ROMPC_UTILS::Target::get_att_euler(double t) {
    return _euler;
}

/**
    @brief Returns attitude axis/angle of FRD target
    w.r.t inertial NED frame
*/
Vec3 ROMPC_UTILS::Target::get_att_aa(double t) {
    return _aa_I_to_R;
}

/**
    @brief Returns target body rates w.r.t inertial
    frame, written in target FRD frame coordinates
*/
Vec3 ROMPC_UTILS::Target::get_om(double t) {
    return _om_R_I_R;
}

/**
    @brief Constructor, initialize glideslope target class

    @param[in] S      target speed [m/s]
    @param[in] gamma  target glideslope angle [rad] (negative = decrease altitude)
    @param[in] th     target pitch angle [rad]
*/
ROMPC_UTILS::SGF::SGF(double S, double gamma, double th): Target(),  _th(th) {
    // Velocity (u,v,w) in target frame coordinates, constant
    _v_r_I_R(0) = S * cos(th - gamma);
    _v_r_I_R(1) = 0.0;
    _v_r_I_R(2) = S * sin(th - gamma);

    // Velocity in inertial coordinates
    _v_r_I_I.setZero();
    _v_r_I_I(2) = S * sin(-gamma); // sink rate, constant
    _S_xy = S * cos(gamma);
}

/**
    @brief Initialize the target position and yaw angle.

    @param[in] p    inertial position [m]
    @param[in] psi  yaw angle [rad]
*/
void ROMPC_UTILS::SGF::initialize(Vec3 p, double psi) {
    // Set initial position
    _p_r_i_I = p;

    // Determine velocities in x and y direction in inertial coord.
    _v_r_I_I(0) = _S_xy * cos(psi);
    _v_r_I_I(1) = _S_xy * sin(psi);

    // Compute orientations
    _euler << 0.0, _th, psi;
    Rot::euler_to_quat(_euler, _q_I_to_R);
    Rot::quat_to_axis(_q_I_to_R, _aa_I_to_R);
}

/**
    @brief Position computed by taking starting position
    and integrating inertial velocity

    @param[in] t  time since initialization [s]
*/
Vec3 ROMPC_UTILS::SGF::get_pos(double t) {
    return _p_r_i_I + t * _v_r_I_I;
}

/**
    @brief Constructor, initialize steady turning flight target class

    @param[in] v      target constant body frame velocity [m/s]
    @param[in] om     target angular velocity in body frame [rad/s]
    @param[in] phi    target roll angle [rad]
    @param[in] th     target pitch angle [rad]
    @param[in] R      target turning radius [m]
*/
ROMPC_UTILS::STF::STF(Vec3 v, Vec3 om, double phi,  double th, double R):
                      Target(), _phi(phi), _th(th), _R(R) {
    _v_r_I_R = v;
    _om_R_I_R = om;
    _psi_dot = v.norm()/R; // compute constant yaw rate
}

/**
    @brief Initialize the target position and yaw angle.

    @param[in] p    inertial position [m]
    @param[in] yaw  initial yaw angle [rad]
*/
void ROMPC_UTILS::STF::initialize(Vec3 p, double psi) {
    _p_r_i_I = p; // initial position
    _psi = psi; // initial yaw angle

    // Compute center of circle, a constant
    _p_c_i_I << -_R*sin(_psi), _R*cos(_psi), 0.0;
    _p_c_i_I += _p_r_i_I;
}

/**
    @brief Position computed by integrating yaw rate

    @param[in] t  time since initialization [s]
*/
Vec3 ROMPC_UTILS::STF::get_pos(double t) {
    double psi = _psi + t * _psi_dot;
    _p_r_i_I << _R*sin(psi), -_R*cos(psi), 0.0;
    _p_r_i_I += _p_c_i_I;
    return _p_r_i_I;
}

/**
    @brief Quaternion attitude of target FRD frame
    w.r.t inertial NED frame

    @param[in] t  time since initialization [s]
*/
void ROMPC_UTILS::STF::compute_euler(double t) {
    double psi = Rot::wrap_to_2pi(_psi + t * _psi_dot);
    _euler << _phi, _th, psi;
}

/**
    @brief Quaternion attitude of target FRD frame
    w.r.t inertial NED frame

    @param[in] t  time since initialization [s]
*/
Vec4 ROMPC_UTILS::STF::get_att_quat(double t) {
    compute_euler(t);
    Rot::euler_to_quat(_euler, _q_I_to_R);
    return _q_I_to_R;
}

/**
    @brief Euler attitude of target FRD frame
    w.r.t inertial NED frame

    @param[in] t  time since initialization [s]
*/
Vec3 ROMPC_UTILS::STF::get_att_euler(double t) {
    compute_euler(t);
    return _euler;
}

/**
    @brief Axis/angle attitude of target FRD frame
    w.r.t inertial NED frame

    @param[in] t  time since initialization [s]
*/
Vec3 ROMPC_UTILS::STF::get_att_aa(double t) {
    compute_euler(t);
    Rot::euler_to_quat(_euler, _q_I_to_R);
    Rot::quat_to_axis(_q_I_to_R, _aa_I_to_R);
    return _aa_I_to_R;
}

/**
    @brief Constructor, initialize OCP object

    @param[in] filepath  path to where the QP matrices are
    @param[in] m         dimension of control vector
*/
ROMPC_UTILS::OCP::OCP(const std::string filepath, const double tmax)
                      : _success(false), _solve_time(0.0), _tmax(tmax) {
    MatX F = Data::load_matrix(filepath + "/F.csv");
    _G = Data::load_matrix(filepath + "/G.csv");
    MatX E1 = Data::load_matrix(filepath + "/E1.csv");
    _e = Data::load_matrix(filepath + "/e.csv");
    _E2 = Data::load_matrix(filepath + "/E2.csv");
    VecX params = Data::load_matrix(filepath + "/ocp_params.csv");

    _nV = E1.cols();
    _nC = E1.rows();
    _n = _G.rows();
    _dt = params(1); // discretization time in seconds
    _N = params(0); // horizon steps

    // Initialize QP problem
    _ocp = qpOASES::QProblem(_nV, _nC);
    qpOASES::Options options;
    options.setToMPC();
    options.printLevel = qpOASES::PL_NONE;
    _ocp.setOptions(options);

    // Define constant problem data structures
    _F = ArrPtr(new qpOASES::real_t[_nV*_nV]);
    _E1 = ArrPtr(new qpOASES::real_t[_nC*_nV]);
    _ub = ArrPtr(new qpOASES::real_t[_nC]);
    _g = ArrPtr(new qpOASES::real_t[_nV]);
    _U = ArrPtr(new qpOASES::real_t[_nV]);

    eigen_to_qpoases(F, _F);
    eigen_to_qpoases(E1, _E1);

    Eigen::VectorXd x0;
    x0.resize(_n);
    x0.setZero();
    set_x0(x0);

    qpOASES::int_t nWSR = 1000;
    qpOASES::real_t cputime = _tmax;
    qpOASES::returnValue st = _ocp.init(_F.get(), _g.get(), _E1.get(), nullptr, nullptr,
                              nullptr, _ub.get(), nWSR, &cputime);

    if (st == qpOASES::SUCCESSFUL_RETURN) _success = true;
    else _success = false;

    _solve_time = cputime;
}

/**
    @brief Converts a Eigen::MatrixXd matrix into an array
    assuming row major ordering used by qpOASES
*/
void ROMPC_UTILS::OCP::eigen_to_qpoases(const MatX& M, ArrPtr& m) {
    RowMajMat::Map(m.get(), M.rows(), M.cols()) = M;
}

/**
    @brief Converts a Eigen::MatrixXd matrix into an array
    assuming row major ordering used by qpOASES
*/
void ROMPC_UTILS::OCP::set_x0(const VecX x0) {
    eigen_to_qpoases(x0.transpose()*_G, _g);
    eigen_to_qpoases(_e + _E2*x0, _ub);
}

/**
    @brief Converts a Eigen::MatrixXd matrix into an array
    assuming row major ordering used by qpOASES

    @param[in] x0        Initial condition
    @param[in] uopt      Writes u_0* to this vector

    @param[out] success  boolean if solved successfully
*/
void ROMPC_UTILS::OCP::solve(const VecX x0, Vec4& uopt) {
    set_x0(x0); // Set the new initial condition

    qpOASES::int_t nWSR = 1000;
    qpOASES::real_t comptime = _tmax;
    qpOASES::returnValue st = _ocp.hotstart(_g.get(), nullptr, nullptr,
                                        nullptr, _ub.get(), nWSR, &comptime);

    if (st == qpOASES::SUCCESSFUL_RETURN) {
        _ocp.getPrimalSolution(_U.get());
        uopt << _U[0], _U[1], _U[2], _U[3];
        _success = true;
    }
    else {
        uopt.setZero();
        _success = false;
    }

    _solve_time = comptime;
}

/**
    @brief Get most recent status of OCP solver
*/
bool ROMPC_UTILS::OCP::success() {
    return _success;
}

/**
    @brief Get most recent QP solve time
*/
double ROMPC_UTILS::OCP::solve_time() {
    return _solve_time;
}

double ROMPC_UTILS::OCP::get_dt() {
    return _dt;
}

int ROMPC_UTILS::OCP::get_N() {
    return _N;
}

/**
    @brief Computes operator T such that om = T(aa) * aa_dot which
    transforms the rate of change of axis/angle representation of
    rotation from A to B into the angular velocity of B w.r.t A written
    in B coordinates.

    @param[in] aa  axis angle representation aa = (th1, th2, th3)
    @param[in] T   transformation operator matrix
*/
void ROMPC_UTILS::tangential_transf(const Vec3& aa, Mat3& T) {
    double th2 = aa.squaredNorm(); // theta squared

    double c1, c2, c3;
    if (th2 < 5e-6) {
        double th4 = th2 * th2;
        c1 = 1.0  - th2/6   + th4/120;  // + O(th^6)
        c2 = 1/2. - th2/24  + th4/720;  // + O(th^6)
        c3 = 1/6. - th2/120 + th4/5040; // + O(th^6)
    }
    else {
        double th = sqrt(th2);
        c1 = sin(th)/th;
        c2 = (1 - cos(th))/th2;
        c3 = (th - sin(th))/(th * th2);
    }
    Mat3 skew_th = (Mat3() << 0.0, -aa(2), aa(1),
                              aa(2), 0.0, -aa(0),
                             -aa(1), aa(0), 0.0).finished();

    T = c1*Mat3::Identity() - c2*skew_th + c3*(aa*aa.transpose());
}

/**
    @brief Converts the angular velocity vector om for a frame B rotating w.r.t A
    into the rate of change of the axis/angle parameters that represent the
    rotation from A to B.

    @param[in] aa     axis/angle repr. of rotation from A to B frames [rad]
    @param[in] om     angular velocity of B w.r.t A [rad/s]
    @param[in] aadot  time derivative of aa [rad/s]
*/
void ROMPC_UTILS::om_to_aadot(const Vec3& aa, const Vec3& om, Vec3& aadot) {
    Mat3 T;
    ROMPC_UTILS::tangential_transf(aa, T);
    aadot = T.inverse() * om;
}

/**
    @brief Given current axis/angle parameters for rotation from A to B and their
    time derivatives, computes the angular velocity vector of B w.r.t A.

    @param[in] aa     axis/angle repr. of rotation from A to B frames [rad]
    @param[in] aadot  time derivative of aa [rad/s]
    @param[in] om     angular velocity of B w.r.t A [rad/s]
*/
void ROMPC_UTILS::aadot_to_om(const Vec3& aa, const Vec3& aadot, Vec3& om) {
    Mat3 T;
    ROMPC_UTILS::tangential_transf(aa, T);
    om = T * aadot;
}


/**********************************************************************
*  Copyright (c) 2008-2013, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_TRANSFORMATION_HPP
#define UTILITIES_GEOMETRY_TRANSFORMATION_HPP

#include <utilities/UtilitiesAPI.hpp>
#include <utilities/data/Matrix.hpp>
#include <utilities/data/Vector.hpp>
#include <utilities/core/Logger.hpp>

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  // forward declaration
  class Point3d;
  class Vector3d;
  class Plane;
  class BoundingBox;
  class EulerAngles;

  class UTILITIES_API Transformation{
  public:

    /// default constructor creates identity transfomation
    Transformation();

    /// constructor from storage, asserts matrix is 4x4
    Transformation(const Matrix& matrix);

    /// constructor from storage, asserts vector is size 16
    Transformation(const Vector& vector);

    /// copy constructor
    Transformation(const Transformation& other);

    /// rotation about origin defined by axis and angle (radians)
    static Transformation rotation(const Vector3d& axis, double radians);

    /// rotation about point defined by axis and angle (radians)
    static Transformation rotation(const Point3d& origin, const Vector3d& axis, double radians);

    /// rotation specified by Euler angles
    static Transformation rotation(const EulerAngles& angles);

    /// translation along vector
    static Transformation translation(const Vector3d& translation);

    /// transforms system with z' to regular system
    /// will try to align y' with z, but if that fails will align y' with y
    static Transformation alignZPrime(const Vector3d& zPrime);

    /// transforms face coordinates to regular system, face normal will be z'
    /// will try to align y' with z, but if that fails will align y' with y
    /// face origin will be minimum point in x', y' and z'=0
    /// will return identity transformation if cannot compute plane for vertices
    static Transformation alignFace(const std::vector<Point3d>& vertices);

    /// returns a transformation which is the inverse of this
    Transformation inverse() const;

    /// get the matrix representation directly
    Matrix matrix() const;

    /// get the vector representation directly
    Vector vector() const;

    /// get the Euler angles for the transformation, does not include translation
    EulerAngles eulerAngles() const;

    /// get the rotation matrix for the transformation, does not include translation
    Matrix rotationMatrix() const;

    /// get the translation for the transformation, does not include rotation
    Vector3d translation() const;

    /// apply the transformation to the point
    Point3d operator*(const Point3d& point) const;

    /// apply the transformation to the vector
    Vector3d operator*(const Vector3d& vector) const;

    /// apply the transformation to the bounding box
    BoundingBox operator*(const BoundingBox& boundingBox) const;

    /// apply the transformation to the plane
    Plane operator*(const Plane& plane) const;

    /// apply the transformation to a vector of points
    std::vector<Point3d> operator*(const std::vector<Point3d>& points) const;

    /// apply the transformation to a vector of vector
    std::vector<Vector3d> operator*(const std::vector<Vector3d>& vectors) const;

    /// apply the transformation to the other transformation
    Transformation operator*(const Transformation& other) const;

  private:

    REGISTER_LOGGER("utilities.Transformation");
    Matrix m_storage;

  };

  /// ostream operator
  UTILITIES_API std::ostream& operator<<(std::ostream& os, const Transformation& t);

  // optional Transformation
  typedef boost::optional<Transformation> OptionalTransformation;

  // vector of Transformation
  typedef std::vector<Transformation> TransformationVector;

  /// create rotation about origin defined by axis and angle (radians)
  UTILITIES_API Transformation createRotation(const Vector3d& axis, double radians);

  /// create rotation about point defined by axis and angle (radians)
  UTILITIES_API Transformation createRotation(const Point3d& origin, const Vector3d& axis, double radians);

  /// create rotation specified by Euler angles
  UTILITIES_API Transformation createRotation(const EulerAngles& angles);

  /// create translation along vector
  UTILITIES_API Transformation createTranslation(const Vector3d& translation);

} // openstudio

#endif //UTILITIES_GEOMETRY_TRANSFORMATION_HPP


/*!
 * @file
 * Contains unit tests for the @ref Monoid typeclass.
 */

#include <boost/mpl11/monoid.hpp>

#include <boost/mpl11/core.hpp>

#include "test_method_dispatch.hpp"

using namespace boost::mpl11;

///////////////////////////
// Test method dispatching
///////////////////////////
namespace boost { namespace mpl11 {
    template <>
    struct Monoid<Archetype<0>> : instantiate<Monoid>::with<Archetype<0>> {
        template <typename, typename> using plus_impl = method_tag<plus>;
        template <typename ...>       using zero_impl = method_tag<zero>;
    };
}}

struct tests
    : method<plus, archetype<0>, archetype<0>>
    // , method<plus, archetype<0>, archetype<1>>
    // , method<plus, archetype<1>, archetype<0>>

    , method<zero, Archetype<0>>
{ };

int main() { }


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/monoid/laws.hpp>
#include <boost/hana/monoid/mcd.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/monoid.hpp>
#include <boost/hana/list/instance.hpp>
using namespace boost::hana;

template <typename mcd>
void test() {
    constexpr auto mono = detail::minimal::monoid<mcd>;
    BOOST_HANA_CONSTEXPR_ASSERT(
        Monoid::laws::check(
            list(mono(0), mono(1), mono(2), mono(3))
        )
    );
}

int main() {
    test<Monoid::mcd>();
}


/* glz_classifier_generator.cc
   Jeremy Barnes, 15 March 2006
   Copyright (c) 2006 Jeremy Barnes  All rights reserved.
   $Source$

   Generator for glz_classifiers.
*/

#include "glz_classifier_generator.h"
#include "registry.h"
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include "training_index.h"
#include "weighted_training.h"
#include "jml/utils/smart_ptr_utils.h"
#include "jml/algebra/matrix_ops.h"
#include "jml/algebra/lapack.h"
#include "jml/arch/timers.h"

using namespace std;

namespace ML {

/*****************************************************************************/
/* GLZ_CLASSIFIER_GENERATOR                                                  */
/*****************************************************************************/

GLZ_Classifier_Generator::
GLZ_Classifier_Generator()
{
    defaults();
}

GLZ_Classifier_Generator::~GLZ_Classifier_Generator()
{
}

void
GLZ_Classifier_Generator::
configure(const Configuration & config)
{
    Classifier_Generator::configure(config);

    config.find(add_bias, "add_bias");
    config.find(do_decode, "decode");
    config.find(link_function, "link_function");
    config.find(normalize, "normalize");
    config.find(ridge_regression, "ridge_regression");
    config.find(feature_proportion, "feature_proportion");
}

void
GLZ_Classifier_Generator::
defaults()
{
    Classifier_Generator::defaults();
    link_function = LOGIT;
    add_bias = true;
    do_decode = true;
    normalize = true;
    ridge_regression = true;
    feature_proportion = 1.0;
}

Config_Options
GLZ_Classifier_Generator::
options() const
{
    Config_Options result = Classifier_Generator::options();
    result
        .add("add_bias", add_bias,
             "add a constant bias term to the classifier?")
        .add("decode", do_decode,
             "run the decoder (link function) after classification?")
        .add("link_function", link_function,
             "which link function to use for the output function");

    return result;
}

void
GLZ_Classifier_Generator::
init(std::shared_ptr<const Feature_Space> fs, Feature predicted)
{
    Classifier_Generator::init(fs, predicted);
    model = GLZ_Classifier(fs, predicted);
}

std::shared_ptr<Classifier_Impl>
GLZ_Classifier_Generator::
generate(Thread_Context & thread_context,
         const Training_Data & training_data,
         const boost::multi_array<float, 2> & weights,
         const std::vector<Feature> & features,
         float & Z,
         int) const
{
    boost::timer timer;

    Feature predicted = model.predicted();

    GLZ_Classifier current(model);

    train_weighted(thread_context, training_data, weights, features, current);

    if (verbosity > 2) {
        cerr << endl << "Learned GLZ function: " << endl;
        cerr << "link: " << current.link << endl;
        int nl = current.feature_space()->info(predicted).value_count();
        cerr << "feature                                    ";
        if (nl == 2 && false)
            cerr << "       label1";
            else
                for (unsigned l = 0;  l < nl;  ++l)
                    cerr << format("    label%-4d", l);
        cerr << endl;

        for (unsigned i = 0;  i < current.features.size() + current.add_bias;
             ++i) {

            if (i == current.features.size()) {
                cerr << format("%-40s", "BIAS");
            }
            else {
                string feat
                    = current.feature_space()
                    ->print(current.features[i].feature);
                cerr << format("%-36s", feat.c_str());

                switch (current.features[i].type) {
                case GLZ_Classifier::Feature_Spec::VALUE:
                    cerr << " VAL";
                    break;
                case GLZ_Classifier::Feature_Spec::VALUE_IF_PRESENT:
                    cerr << " VIP";
                    break;
                case GLZ_Classifier::Feature_Spec::PRESENCE:
                    cerr << " PRS";
                    break;
                default:
                    throw Exception("invalid type");
                }
            }

            if (nl == 2 && false)
                cerr << format("%13f", current.weights[1][i]);
            else
                for (unsigned l = 0;  l < nl;  ++l)
                    cerr << format("%13f", current.weights[l][i]);
            cerr << endl;
        }
        cerr << endl;
    }

    Z = 0.0;

    return make_sp(current.make_copy());
}

float
GLZ_Classifier_Generator::
train_weighted(Thread_Context & thread_context,
               const Training_Data & data,
               const boost::multi_array<float, 2> & weights,
               const std::vector<Feature> & unfiltered,
               GLZ_Classifier & result) const
{
    /* Algorithm:
       1.  Convert training data to a dense format;
       2.  Train on each column
    */

    result = model;
    result.features.clear();
    result.add_bias = add_bias;
    result.link = (do_decode ? link_function : LINEAR);

    Feature predicted = model.predicted();

    for (unsigned i = 0;  i < unfiltered.size();  ++i) {
        if (unfiltered[i] == model.predicted())
            continue;  // don't use the label to predict itself

        // If we don't want to use all features then take a random subset
        if (feature_proportion < 1.0
            && thread_context.random01() > feature_proportion)
            continue;

        GLZ_Classifier::Feature_Spec spec(unfiltered[i]);

        // Can't use a feature that has multiple occurrences
        if (!data.index().only_one(unfiltered[i])) continue;

        if (data.index().exactly_one(unfiltered[i])) {
            // Feature that's always there but constant has no information
            if (data.index().constant(unfiltered[i])) continue;
            result.features.push_back(spec);
        }
        else {
            if (!data.index().constant(unfiltered[i])) {
                spec.type = GLZ_Classifier::Feature_Spec::VALUE_IF_PRESENT;
                result.features.push_back(spec);
            }
            spec.type = GLZ_Classifier::Feature_Spec::PRESENCE;
            result.features.push_back(spec);
        }
    }

    size_t nl = result.label_count();        // Number of labels
    bool regression_problem = (nl == 1);
    size_t nx = data.example_count();        // Number of examples
    size_t nv = result.features.size();      // Number of variables
    if (add_bias) ++nv;

    distribution<double> total_weight(nx);
    for (unsigned x = 0;  x < nx;  ++x) {
        for (unsigned l = 0;  l < weights.shape()[1];  ++l)
            total_weight[x] += weights[x][l];
    }

    size_t nx2 = (total_weight != 0.0).count();

    //cerr << "nx = " << nx << " nv = " << nv << " nx * nv = " << nx * nv
    //     << endl;

    Timer t;

    /* Get the labels by example. */
    const vector<Label> & labels = data.index().labels(predicted);

    // Use double precision, we have enough memory (<= 1GB)
    // NOTE: always on due to issues with convergence
    boost::multi_array<double, 2> dense_data(boost::extents[nv][nx2]);  // training data, dense

    distribution<double> model(nx2, 0.0);  // to initialise weights, correct
    vector<distribution<double> > w(nl, model);       // weights for each label
    vector<distribution<double> > correct(nl, model); // correct values

    cerr << "setup: " << t.elapsed() << endl;
    t.restart();

    int x2 = 0;
    for (unsigned x = 0;  x < nx;  ++x) {
        if (total_weight[x] == 0.0) continue;

        distribution<float> decoded = result.decode(data[x]);
        if (add_bias) decoded.push_back(1.0);

        //cerr << "x = " << x << "  decoded = " << decoded << endl;

        /* Record the values of the variables. */
        assert(decoded.size() == nv);
        for (unsigned v = 0;  v < decoded.size();  ++v) {
            if (!isfinite(decoded[v])) decoded[v] = 0.0;
            dense_data[v][x2] = decoded[v];
        }

        /* Record the correct label. */
        if (regression_problem) {
            correct[0][x2] = labels[x].value();
            w[0][x2] = weights[x][0];
        }
        else if (nl == 2 && weights.shape()[1] == 1) {
            correct[0][x2] = (double)(labels[x] == 0);
            correct[1][x2] = (double)(labels[x] == 1);
            w[0][x2] = weights[x][0];
        }
        else {
            for (unsigned l = 0;  l < nl;  ++l) {
                correct[l][x2] = (double)(labels[x] == l);
                w[l][x2] = weights[x][l];
            }
        }

        ++x2;
    }

    if (x2 != nx2)
        throw Exception("x2 not nx2");

    cerr << "marshalling: " << t.elapsed() << endl;
    t.restart();

    distribution<double> means(nv), stds(nv, 1.0);

    /* Scale */
    for (unsigned v = 0;  v < nv && normalize;  ++v) {
        double total = 0.0;

        for (unsigned x = 0;  x < nx2;  ++x)
            total += dense_data[v][x];

        double mean = total / nx2;

        double std_total = 0.0;
        for (unsigned x = 0;  x < nx2;  ++x)
            std_total
                += (dense_data[v][x] - mean)
                *  (dense_data[v][x] - mean);

        double std = sqrt(std_total / nx2);

        if (std == 0.0 && mean == 1.0) {
            // bias column
            std = 1.0;
            mean = 0.0;
        }
        else if (std == 0.0)
            std = 1.0;

        double std_recip = 1.0 / std;

        for (unsigned x = 0;  x < nx2;  ++x)
            dense_data[v][x] = (dense_data[v][x] - mean) * std_recip;

        means[v] = mean;
        stds[v] = std;
    }

    cerr << "normalization: " << t.elapsed() << endl;
    t.restart();

    int nlr = nl;
    if (nl == 2) nlr = 1;

    /* Perform a GLZ for each label. */
    result.weights.clear();
    double extra_bias = 0.0;
    for (unsigned l = 0;  l < nlr;  ++l) {
        //cerr << "l = " << l << "  correct[l] = " << correct[l]
        //     << " w = " << w[l] << endl;

        distribution<double> trained
            = perform_irls(correct[l], dense_data, w[l], link_function,
                           ridge_regression);

        trained /= stds;
        extra_bias = - (trained.dotprod(means));

        if (extra_bias != 0.0) {
            if (!add_bias)
                throw Exception("extra bias but nowhere to put it");
            trained.back() += extra_bias;
        }

        //cerr << "l = " << l <<"  param = " << param << endl;

        result.weights.push_back(trained.cast<float>());
    }

    cerr << "irls: " << t.elapsed() << endl;
    t.restart();

    if (nl == 2) {
        // weights for second label are the mirror of those of the first
        // label
        result.weights.push_back(-1.0F * result.weights.front());
    }

    //cerr << "glz_classifier: irls time " << t.elapsed() << "s" << endl;

    return 0.0;
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Generator, GLZ_Classifier_Generator>
    GLZ_CLASSIFIER_REGISTER("glz");

} // file scope

} // namespace ML


//=======================================================================
// Copyright 2001 University of Notre Dame.
// Author: Jeremy G. Siek
//
// 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)
//=======================================================================

#include <boost/config.hpp>
#include <boost/core/lightweight_test.hpp>
#include <stdlib.h>

#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_archetypes.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/random.hpp>

#include <boost/random/mersenne_twister.hpp>

template < typename ColorMap, typename ParentMap, typename DiscoverTimeMap,
    typename FinishTimeMap >
class dfs_test_visitor
{
    typedef typename boost::property_traits< ColorMap >::value_type ColorValue;
    typedef typename boost::color_traits< ColorValue > Color;

public:
    dfs_test_visitor(
        ColorMap color, ParentMap p, DiscoverTimeMap d, FinishTimeMap f)
    : m_color(color)
    , m_parent(p)
    , m_discover_time(d)
    , m_finish_time(f)
    , m_time(0)
    {
    }

    template < class Vertex, class Graph >
    void initialize_vertex(Vertex u, Graph&)
    {
        BOOST_TEST(boost::get(m_color, u) == Color::white());
    }
    template < class Vertex, class Graph > void start_vertex(Vertex u, Graph&)
    {
        BOOST_TEST(boost::get(m_color, u) == Color::white());
    }
    template < class Vertex, class Graph >
    void discover_vertex(Vertex u, Graph&)
    {
        using namespace boost;
        BOOST_TEST(get(m_color, u) == Color::gray());
        BOOST_TEST(get(m_color, get(m_parent, u)) == Color::gray());

        put(m_discover_time, u, m_time++);
    }
    template < class Edge, class Graph > void examine_edge(Edge e, Graph& g)
    {
        using namespace boost;
        BOOST_TEST(get(m_color, source(e, g)) == Color::gray());
    }
    template < class Edge, class Graph > void tree_edge(Edge e, Graph& g)
    {
        using namespace boost;
        BOOST_TEST(get(m_color, target(e, g)) == Color::white());

        put(m_parent, target(e, g), source(e, g));
    }
    template < class Edge, class Graph > void back_edge(Edge e, Graph& g)
    {
        using namespace boost;
        BOOST_TEST(get(m_color, target(e, g)) == Color::gray());
    }
    template < class Edge, class Graph >
    void forward_or_cross_edge(Edge e, Graph& g)
    {
        using namespace boost;
        BOOST_TEST(get(m_color, target(e, g)) == Color::black());
    }
    template < class Edge, class Graph > void finish_edge(Edge e, Graph& g)
    {
        using namespace boost;
        BOOST_TEST((get(m_color, target(e, g)) == Color::gray())
            || (get(m_color, target(e, g)) == Color::black()));
    }
    template < class Vertex, class Graph > void finish_vertex(Vertex u, Graph&)
    {
        using namespace boost;
        BOOST_TEST(get(m_color, u) == Color::black());

        put(m_finish_time, u, m_time++);
    }

private:
    ColorMap m_color;
    ParentMap m_parent;
    DiscoverTimeMap m_discover_time;
    FinishTimeMap m_finish_time;
    typename boost::property_traits< DiscoverTimeMap >::value_type m_time;
};

template < typename Graph > struct dfs_test
{
    typedef boost::graph_traits< Graph > Traits;
    typedef typename Traits::vertices_size_type vertices_size_type;

    static void go(vertices_size_type max_V)
    {
        using namespace boost;
        typedef typename Traits::vertex_descriptor vertex_descriptor;
        typedef
            typename boost::property_map< Graph, boost::vertex_color_t >::type
                ColorMap;
        typedef
            typename boost::property_traits< ColorMap >::value_type ColorValue;
        typedef typename boost::color_traits< ColorValue > Color;
        typedef typename boost::property_map< Graph, boost::edge_color_t >::type
            EColorMap;
        typedef typename boost::property_traits< EColorMap >::value_type
            EColorValue;
        typedef typename boost::color_traits< EColorValue > EColor;

        vertices_size_type i, k;
        typename Traits::edges_size_type j;
        typename Traits::vertex_iterator vi, vi_end, ui, ui_end;
        typename Traits::edge_iterator ei, ei_end;

        boost::mt19937 gen;

        for (i = 0; i < max_V; ++i)
            for (j = 0; j < i * i; ++j)
            {
                Graph g;
                generate_random_graph(g, i, j, gen);

                ColorMap color = get(boost::vertex_color, g);
                EColorMap e_color = get(boost::edge_color, g);
                std::vector< vertex_descriptor > parent(num_vertices(g));
                for (k = 0; k < num_vertices(g); ++k)
                    parent[k] = k;
                std::vector< int > discover_time(num_vertices(g)),
                    finish_time(num_vertices(g));

                // Get vertex index map
                typedef typename boost::property_map< Graph,
                    boost::vertex_index_t >::const_type idx_type;
                idx_type idx = get(boost::vertex_index, g);

                typedef boost::iterator_property_map<
                    typename std::vector< vertex_descriptor >::iterator,
                    idx_type >
                    parent_pm_type;
                parent_pm_type parent_pm(parent.begin(), idx);
                typedef boost::iterator_property_map<
                    std::vector< int >::iterator, idx_type >
                    time_pm_type;
                time_pm_type discover_time_pm(discover_time.begin(), idx);
                time_pm_type finish_time_pm(finish_time.begin(), idx);

                dfs_test_visitor< ColorMap, parent_pm_type, time_pm_type,
                    time_pm_type >
                    vis(color, parent_pm, discover_time_pm, finish_time_pm);

                boost::undirected_dfs(
                    g, visitor(vis).color_map(color).edge_color_map(e_color));

                // all vertices should be black
                for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
                    BOOST_TEST(get(color, *vi) == Color::black());

                // all edges should be black
                for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
                    BOOST_TEST(get(e_color, *ei) == EColor::black());

                // check parenthesis structure of discover/finish times
                // See CLR p.480
                for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
                    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end;
                         ++vi)
                    {
                        vertex_descriptor u = *ui, v = *vi;
                        if (u != v)
                        {
                            BOOST_TEST(finish_time[u] < discover_time[v]
                                || finish_time[v] < discover_time[u]
                                || (discover_time[v] < discover_time[u]
                                    && finish_time[u] < finish_time[v]
                                    && boost::is_descendant(u, v, parent_pm))
                                || (discover_time[u] < discover_time[v]
                                    && finish_time[v] < finish_time[u]
                                    && boost::is_descendant(v, u, parent_pm)));
                        }
                    }
            }
    }
};

// usage: undirected_dfs.exe [max-vertices=15]

int main(int argc, char* argv[])
{
    int max_V = 7;
    if (argc > 1)
        max_V = atoi(argv[1]);

    // Test undirected graphs.
    dfs_test< boost::adjacency_list< boost::vecS, boost::vecS,
        boost::undirectedS,
        boost::property< boost::vertex_color_t, boost::default_color_type >,
        boost::property< boost::edge_color_t, boost::default_color_type > > >::
        go(max_V);

    return boost::report_errors();
}


#include <armadillo>
#include <h5cpp/all>
#include <random>

int main(){

	h5::fd_t fd = h5::create("example.h5",H5F_ACC_TRUNC);

	std::random_device rd;
	std::mt19937 gen(rd());
	std::uniform_int_distribution<> dist(40, 45);

	arma::SpMat<int>M(20,40);
	for(int i = 0, j=dist(gen); i<M.n_elem; i += j)
	   	M[i] = j, j = dist(gen);
	//std::cout << M;
	std::cout <<"size: " << M.n_rows << "x" << M.n_cols
		<<" state: " << M.vec_state // 0 matrix, 1 colvec, 2 rowvec
	   	<<" non zeros: " <<  M.n_nonzero
		<< " fill rate: " <<  (double)M.n_nonzero / (double)M.n_cols *  (double)M.n_rows   <<"\n";

	auto extra_attributes = std::make_tuple(
		"author", "Steven Varga",  "company","vargaconsulting",  "date", "2019-oct-17");

	/* single IO op to write a sparse matrix
	 */
	h5::gr_t gr = h5::write(fd, "sparse-multi-file.plain", M );
	h5::awrite(gr,extra_attributes);

	/* compression and sub setting is supported, although the interpretation is delegated to software writer
	 * generally it is suggested to use single IO calls instead of chunked access
	 * */
	h5::write(fd, "sparse-multi-file.gzip", M, h5::chunk{254} | h5::gzip{9}, h5::offset{1024});
	arma::mat K(4,4);

	auto spmat = h5::read<arma::sp_mat>(gr);
	std::cout<<"is dense: " << h5::exp::linalg::is_dense<decltype(K)>::value <<"\n";
	std::cout<<"is continuous: " << h5::exp::is_contigious<decltype(K)>::value <<"\n";
	std::cout<<"rank: " << h5::exp::rank<int>::value << "\n";
	std::cout<<"rank: " << h5::exp::rank<std::vector<int>>::value << "\n";

	//std::cout << values <<"\n";
}


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_dae_analytical.hpp>
#include <test/unit/math/rev/functor/dae_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using dae_test_types = boost::mp11::mp_product<
    ode_test_tuple, ::testing::Types<dae_functor>,
    ::testing::Types<double>,                                  // t
    ::testing::Types<double, stan::math::var_value<double> >,  // yy0
    ::testing::Types<double, stan::math::var_value<double> >,  // yp0
    ::testing::Types<double, stan::math::var_value<double> >   // theta
    >;

TYPED_TEST_SUITE_P(analytical_dae_test);
TYPED_TEST_P(analytical_dae_test, dv) {
  double k = 0.5;
  double t = 5;
  this->dae_sol_dv.ts = std::vector<double>{t};
  Eigen::VectorXd x(1);
  x << k;
  this->test_analytical(this->dae_sol_dv, this->analy_sol_functor(),
                        this->analy_grad_theta_functor(), 1e-8, x, t, k);
}

TYPED_TEST_P(analytical_dae_test, vd) {
  double k = 0.5;
  double t = 5;
  this->dae_sol_vd.ts = std::vector<double>{t};
  this->dae_sol_vd.theta = k;
  Eigen::VectorXd x(3);
  x << 1.0, 0.0, 0.0;
  this->dae_sol_vd.yy0 = x;
  this->test_analytical(this->dae_sol_vd, this->analy_sol_functor(),
                        this->analy_grad_yy0_functor(), 1e-8, x, t, k);
}

REGISTER_TYPED_TEST_SUITE_P(analytical_dae_test, dv, vd);
INSTANTIATE_TYPED_TEST_SUITE_P(StanDAE, analytical_dae_test, dae_test_types);


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <algorithm>
using std::min;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Row;
using arma::rowvec;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenRowVecGenRowVec : public Expected {
    public:
      ExpectedInPlaceGenRowVecGenRowVec() {
        cout << "Compute ExpectedInPlaceGenRowVecGenRowVec(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenRowVec,
            InputClass::GenRowVec
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genRowVecA = *static_cast<Row<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _genRowVecB = *static_cast<Row<double>*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenRowVecA = _genRowVecA;
            _copyOfGenRowVecB = _genRowVecB;

            expectedRowInPlaceEqual();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowInPlacePlus();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowInPlaceMinus();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowInPlaceTimes();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowInPlaceElemTimes();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowInPlaceElemDivide();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowSwap();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowCopy_size();

            _genRowVecA = _copyOfGenRowVecA;
            _genRowVecB = _copyOfGenRowVecB;
            expectedRowRow();
          }

          cout << "done." << endl;
        }

    protected:
      Row<double> _genRowVecA;
      Row<double> _copyOfGenRowVecA;

      Row<double> _genRowVecB;
      Row<double> _copyOfGenRowVecB;

      void expectedRowInPlaceEqual() {
    	if(_genRowVecA.n_cols != _genRowVecB.n_cols || _genRowVecA.n_rows != _genRowVecB.n_rows) {
          return;
        }
        cout << "- Compute expectedRowInPlaceEqual() ... ";

        _genRowVecA = _genRowVecB;
        save<double>("Row.inPlaceEqual", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowInPlacePlus() {
        if(_genRowVecA.n_cols != _genRowVecB.n_cols || _genRowVecA.n_rows != _genRowVecB.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlace() ... ";

        _genRowVecA += _genRowVecB;
        save<double>("Row.inPlacePlus", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowInPlaceMinus() {
        if(_genRowVecA.n_cols != _genRowVecB.n_cols || _genRowVecA.n_rows != _genRowVecB.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlaceMinus() ... ";

        _genRowVecA -= _genRowVecB;
        save<double>("Row.inPlaceMinus", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowInPlaceTimes() {
        if(_genRowVecA.n_cols != _genRowVecB.n_rows) {
          return;
        }
        if(_genRowVecA.n_cols != _genRowVecB.n_cols) {
          return;
        }

        cout << "- Compute expectedRowInPlaceTimes() ... ";

        _genRowVecA *= _genRowVecB;
        save<double>("Row.inPlaceTimes", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowInPlaceElemTimes() {
        if(_genRowVecA.n_cols != _genRowVecB.n_cols || _genRowVecA.n_rows != _genRowVecB.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlaceElemTimes() ... ";

        _genRowVecA %= _genRowVecB;
        save<double>("Row.inPlaceElemTimes", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowInPlaceElemDivide() {
        if(_genRowVecA.n_cols != _genRowVecB.n_cols || _genRowVecA.n_rows != _genRowVecB.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlaceElemDivide() ... ";

        _genRowVecA /= _genRowVecB;
        save<double>("Row.inPlaceElemDivide", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowSwap() {
        if(!_genRowVecB.is_rowvec()) {
          return;
        }

        cout << "- Compute expectedRowSwap() ... ";

        _genRowVecA.swap(_genRowVecB);
        save<double>("Row.swap", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowCopy_size() {
        if(!_genRowVecB.is_rowvec()) {
          return;
        }
        cout << "- Compute expectedRowCopy_size() ... ";

        _genRowVecA.copy_size(_genRowVecB);
        save<double>("Row.copy_size", _genRowVecA);

        cout << "done." << endl;
      }

      void expectedRowRow() {
        if(!_genRowVecB.is_rowvec()) {
          return;
        }

        cout << "- Compute expectedRowRow() ... ";

        _genRowVecA = rowvec(_genRowVecB);
        save<double>("Row.row", _genRowVecA);

        cout << "done." << endl;
      }
  };
}


#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace ipc {

// Boolean scalar
typedef Eigen::Array<bool, Eigen::Dynamic, 1> ArrayXb;
typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;
typedef Eigen::Matrix<bool, 3, 1> Vector3b;
typedef Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> MatrixXb;

template <typename T, int dim, int max_dim = dim>
using Vector = Eigen::Matrix<T, dim, 1, Eigen::ColMajor, max_dim, 1>;
template <typename T, int dim, int max_dim = dim>
using RowVector = Eigen::Matrix<T, 1, dim, Eigen::RowMajor, 1, max_dim>;
template <typename T> using Vector1 = Vector<T, 1>;
template <typename T> using Vector2 = Vector<T, 2>;
template <typename T> using Vector3 = Vector<T, 3>;
template <typename T> using VectorX = Vector<T, Eigen::Dynamic>;
template <typename T> using RowVector2 = RowVector<T, 2>;
template <typename T> using RowVector3 = RowVector<T, 3>;
template <typename T> using RowVectorX = RowVector<T, Eigen::Dynamic>;
template <typename T> using Matrix2 = Eigen::Matrix<T, 2, 2>;
template <typename T> using Matrix3 = Eigen::Matrix<T, 3, 3>;
template <typename T>
using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

using Vector1d = Vector1<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
template <typename T> using VectorMax2 = Vector<T, Eigen::Dynamic, 2>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
template <typename T> using VectorMax3 = Vector<T, Eigen::Dynamic, 3>;
/// @brief A dynamic size matrix with a fixed maximum size of 2 x 1
using VectorMax2d = VectorMax2<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
using VectorMax3d = VectorMax3<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 1
using VectorMax3i = VectorMax3<int>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 1
template <typename T> using VectorMax6 = Vector<T, Eigen::Dynamic, 6>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 1
using VectorMax6d = VectorMax6<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 1
using VectorMax6b = VectorMax6<bool>;
/// @brief A dynamic size matrix with a fixed maximum size of 9 x 1
template <typename T> using VectorMax9 = Vector<T, Eigen::Dynamic, 9>;
/// @brief A dynamic size matrix with a fixed maximum size of 9 x 1
using VectorMax9d = VectorMax9<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 1
template <typename T> using VectorMax12 = Vector<T, Eigen::Dynamic, 12>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 1
using VectorMax12d = VectorMax12<double>;
template <typename T, int max_rows, int max_cols>
using MatrixMax = Eigen::Matrix<
    T,
    Eigen::Dynamic,
    Eigen::Dynamic,
    Eigen::ColMajor,
    max_rows,
    max_cols>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
template <typename T> using MatrixMax2 = MatrixMax<T, 2, 2>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
template <typename T> using MatrixMax3 = MatrixMax<T, 3, 3>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 6
template <typename T> using MatrixMax6 = MatrixMax<T, 6, 6>;
/// @brief A dynamic size matrix with a fixed maximum size of 9 x 9
template <typename T> using MatrixMax9 = MatrixMax<T, 9, 9>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 12
template <typename T> using MatrixMax12 = MatrixMax<T, 12, 12>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
using MatrixMax2d = MatrixMax2<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 3 x 3
using MatrixMax3d = MatrixMax3<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 6 x 6
using MatrixMax6d = MatrixMax6<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 12
using MatrixMax9d = MatrixMax9<double>;
/// @brief A dynamic size matrix with a fixed maximum size of 12 x 12
using MatrixMax12d = MatrixMax12<double>;
/// @brief A dynamic size diagonal matrix
using DiagonalMatrixXd = Eigen::DiagonalMatrix<double, Eigen::Dynamic>;
/// @brief A dynamic size diagonal matrix with a fixed maximum size of 6 x 6
using DiagonalMatrixMax6d = Eigen::DiagonalMatrix<double, Eigen::Dynamic, 6>;

/// @brief A dynamic size array with a fixed maximum size of 2 x 1
template <typename T>
using ArrayMax2 = Eigen::Array<T, Eigen::Dynamic, 1, Eigen::ColMajor, 2, 1>;
/// @brief A dynamic size array with a fixed maximum size of 2 x 1
template <typename T>
using ArrayMax3 = Eigen::Array<T, Eigen::Dynamic, 1, Eigen::ColMajor, 3, 1>;
/// @brief A dynamic size array with a fixed maximum size of 2 x 1
using ArrayMax2d = ArrayMax2<double>;
/// @brief A dynamic size array with a fixed maximum size of 3 x 1
using ArrayMax3d = ArrayMax3<double>;
/// @brief A dynamic size array with a fixed maximum size of 3 x 1
using ArrayMax3i = ArrayMax3<int>;

/// Matrix Projection onto Positive Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_pd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& A,
    double eps = 1e-8);
/// Matrix Projection onto Positive Semi-Definite Cone
template <
    typename _Scalar,
    int _Rows,
    int _Cols,
    int _Options,
    int _MaxRows,
    int _MaxCols>
Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
project_to_psd(
    const Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>&
        A);

/// Cross product for dynamically sized matrices.
template <
    typename DerivedA,
    typename DerivedB,
    typename Result = Eigen::Matrix<
        typename DerivedA::Scalar,
        DerivedA::RowsAtCompileTime,
        DerivedA::ColsAtCompileTime>>
Result cross(
    const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b);

} // namespace ipc

#include "eigen_ext.tpp"


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/console/parse.h>
#include <algorithm>
#include <functional>
#include <math.h>
#include <cstdlib>
#include <string>
#include <map>
#include "../include/helper.h"
#include "../include/segmentation.h"
#include "../include/cuda_methods.h"
#include "../include/preprocess.h"
#include <boost/program_options.hpp>
#include "../include/cycleTimer.h"

int loadPC(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc, std::string filename){

  if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *pc) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  return (1);
}

void showhelpinfo(char *s)
{
  cout<<"Usage:   "<<s<<" [-option] [argument]"<<endl;
  cout<<"option:  "<<"-h  show help information"<<endl;
  cout<<"         "<<"-n number of points"<<endl;
  cout<<"         "<<"-m number of samples"<<endl;
  cout<<"         "<<"-a voxelization grid size in sampling step"<<endl;
  cout<<"         "<<"-b voxelization grid size in segmentation step"<<endl;
}

int
main (int argc, char** argv)
{

  // top-level parameters:

  int num_samples_in;
  float grid_size_1;
  float grid_size_2;
  int num_pts_in;
  int help_bool = 0;
  int full_file = 1;
  // default:

  if(argc == 1)
    {
      printf("no args!, running default parameters \n");
      num_samples_in = 100000;
      grid_size_1 = 0.02;
      grid_size_2 = 0.12;
      full_file = 0;
      num_pts_in = 1000000;
      cout<<" number of pts input: "<<num_pts_in<<endl;
      cout<<" number of samples: "<<num_samples_in<<endl;
      cout<<" grid size in sampling: "<<grid_size_1<<endl;
      cout<<" grid size in segmentation: "<<grid_size_2<<endl;
    }
  else{
  char tmp;
  while((tmp=getopt(argc,argv,"hn:m:a:b:"))!=-1)
    {
      switch(tmp)
	{
	  /*option h : help infomation*/
	case 'h':
	  showhelpinfo(argv[0]);
	  help_bool = 1;
	  break;
      /*option n : set number of input points to read */
    case 'n':
      num_pts_in = atoi(optarg);
	  full_file = 0;
      cout<<" number of pts input: "<<optarg<<endl;
      break;
	  /*option m : set number of samples */
	case 'm':
	  num_samples_in = atoi(optarg);
	  cout<<" number of samples: "<<optarg<<endl;
	  break;
	  /*option a : set sampling grid size*/
	case 'a':
	  grid_size_1 = atof(optarg);
	  cout<<" grid size in sampling: "<<optarg<<endl;
	  break;
	  /*option b: set segmentation grid size */
	case 'b':
	  grid_size_2 = atof(optarg);
	  cout<<" grid size in segmentation: "<<optarg<<endl;
      break;
	default:
	  showhelpinfo(argv[0]);
	  break;
	}
    }
  }

  if(help_bool == 1){
    return(0);
  }

  double readTime = 0.f;
  double samplingPreprocessTime = 0.f;
  double samplingTime = 0.f;
  double sampledWriteTime = 0.f;
  double segmentPreprocessTime = 0.f;
  double segmentTime = 0.f;
  double totalTime = 0.f;

  double startTime = CycleTimer::currentSeconds();

  printf("reading point cloud file! \n");
  //std::string filename("/afs/andrew.cmu.edu/usr18/ardras/private/15-618/pcseg/cuda/data/sample.pcd");
  std::string filename("/afs/andrew.cmu.edu/usr18/ardras/data/kitchen_small_1.pcd");
  //std::string filename("/afs/andrew.cmu.edu/usr18/rohanv/data/kitchen_small_1.pcd");

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc (new pcl::PointCloud<pcl::PointXYZRGB>);

  if(loadPC(pc,filename)){
    std::cout << "Loaded "
	      << pc->width * pc->height
	      << " data points from kitchen_small_1.pcd"
	      << std::endl;
  }

  printf("finished reading point cloud file! \n");
  double endReadTime = CycleTimer::currentSeconds();

  /************* STAGE 0 : PREPROCESS POINT CLOUD **************/
  if(full_file == 1){
    num_pts_in = pc->size();
    cout<<" number of pts_input: "<<num_pts_in<<endl;
  }

  int num_pts = num_pts_in;
  float x_grid = grid_size_1;
  float y_grid = grid_size_1;
  float z_grid = grid_size_1;
  int x_idx;
  int y_idx;
  int z_idx;
  int yz_idx;
  int num_voxels;
  std::vector<std::vector<float> > voxel_pointXYZ;
  std::vector<std::vector<float> > voxel_pointRGB;
  std::vector<std::vector<float> > voxel_pdensity_1;
  float* pdensity_dummy = new float[num_pts]();
  float* pdensity_dummy2 = new float[num_pts]();
  preprocess_step1(num_pts,pc,pdensity_dummy,x_grid,y_grid,z_grid,num_voxels,x_idx,y_idx,z_idx,yz_idx,
		   voxel_pointXYZ,voxel_pointRGB,voxel_pdensity_1);

  float* flattenXYZ = (float*) malloc(num_pts * 3 * sizeof(float));
  float* flattenRGB = (float*) malloc(num_pts * 3 * sizeof(float));
  int* voxel_offset = (int*) malloc( sizeof(int) * (num_voxels+1));
  int* neighbor_ids = (int*) malloc(sizeof(int) * 7 * num_voxels);
  preprocess_step2(voxel_pointXYZ,voxel_pointRGB,voxel_pdensity_1,x_idx,y_idx,z_idx,yz_idx,num_voxels,
		   flattenXYZ,flattenRGB,pdensity_dummy2,voxel_offset,neighbor_ids);

  double endSamplingPreprocessTime = CycleTimer::currentSeconds();

  /************* STAGE 1 : SAMPLING ************/
  /* STEP 1:
     For all i, points:
     In single step, compute (iterative sum/reduction) for all j neighbors of i, A_i,j * x_j, and sum. then || x_i - sum || is weight
     A_i,j is a kernel: RBF/Gaussian/Epachanikov. Store in importance vector. Normalize.
     Simultaneously compute density. (can do this later since only need this for sampled points)
  */
  /* STEP 2:
     Randomly Sample K points according to weights , and construct new resampled pointcloud.
  */

    int num_samples = num_samples_in;
    uint* samples_arr = (uint*)malloc(num_samples* sizeof(uint));
    float* pdens = (float*) malloc(num_samples * sizeof(float));

    cuda_resampling(num_pts, num_voxels, flattenXYZ,flattenRGB,voxel_offset,
	       neighbor_ids,x_idx,y_idx,z_idx,num_samples,samples_arr,pdens);

    double endSamplingTime = CycleTimer::currentSeconds();

    // construct resampled point cloud from sample indices:
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_rs (new pcl::PointCloud<pcl::PointXYZRGB>);
    pc_rs->width = num_samples;
    pc_rs->height = 1;
    pc_rs->points.resize(pc_rs->height * pc_rs->width);

    for (int i =0 ; i < num_samples ;i++){
      pc_rs->points[i].x = flattenXYZ[samples_arr[i]*3];
      pc_rs->points[i].y = flattenXYZ[(3*samples_arr[i])+1];
      pc_rs->points[i].z = flattenXYZ[(3*samples_arr[i])+2];
      pc_rs->points[i].r = (uint8_t)floor((flattenRGB[samples_arr[i]*3] * 255.0));
      pc_rs->points[i].g = (uint8_t)floor((flattenRGB[(samples_arr[i]*3)+1] *255.0));
      pc_rs->points[i].b = (uint8_t)floor((flattenRGB[(samples_arr[i]*3)+2] *255.0));
    }

    // write resampled point cloud pc_rs to pcd file
    pcl::io::savePCDFileASCII ("rs_pcd.pcd", *pc_rs);
    std::cerr << "Saved " << pc_rs->size () << " data points to resampled point cloud rs_pcd.pcd." << std::endl;

    double endWriteResampledTime = CycleTimer::currentSeconds();

    /************* STAGE 3 : PREPROCESS POINT CLOUD **************/
    int num_pts_rs = pc_rs->size();
    float x_grid_rs = grid_size_2;
    float y_grid_rs = grid_size_2;
    float z_grid_rs = grid_size_2;
    int x_idx_rs;
    int y_idx_rs;
    int z_idx_rs;
    int yz_idx_rs;
    int num_voxels_rs;
    std::vector<std::vector<float> > voxel_pointXYZ_rs;
    std::vector<std::vector<float> > voxel_pointRGB_rs;
    std::vector<std::vector<float> > voxel_pdensity_rs;

    preprocess_step1(num_pts_rs,pc_rs,pdens,x_grid_rs,y_grid_rs,z_grid_rs,num_voxels_rs,
		     x_idx_rs,y_idx_rs,z_idx_rs,yz_idx_rs,voxel_pointXYZ_rs,voxel_pointRGB_rs,voxel_pdensity_rs);

    float* flattenXYZ_rs = (float*) malloc(num_pts_rs * 3 * sizeof(float));
    float* flattenRGB_rs = (float*) malloc(num_pts_rs * 3 * sizeof(float));
    float* pdens_new = (float*) malloc(num_pts_rs * sizeof(float));
    int* voxel_offset_rs = (int*) malloc( sizeof(int) * (num_voxels_rs+1));
    int* neighbor_ids_rs = (int*) malloc(sizeof(int) * 7 * num_voxels_rs);
    int* parents = (int*) malloc(num_pts_rs * sizeof(int));
    preprocess_step2(voxel_pointXYZ_rs,voxel_pointRGB_rs,voxel_pdensity_rs,x_idx_rs,y_idx_rs,z_idx_rs,
		     yz_idx_rs,num_voxels_rs,flattenXYZ_rs,flattenRGB_rs,pdens_new,voxel_offset_rs,neighbor_ids_rs);

    double endSegmentPreprocessTime = CycleTimer::currentSeconds();

    /*************** STAGE 4 : SEGMENTATION ***************/
    /* STEP 3:
     * Compute P_n = sum A'_{i,j} for all i in resampled point cloud. A'_{i,j} is another kernel.
     * STEP 4:
     * Do segmentation procedure for each i and link to closest parent with a higher density.
     ******************************************************/

    cuda_segmentation(num_pts_rs, num_voxels_rs,pdens_new, flattenXYZ_rs,flattenRGB_rs,voxel_offset_rs,
			neighbor_ids_rs,x_idx_rs,y_idx_rs,z_idx_rs,parents);

    double endTime = CycleTimer::currentSeconds();

    /*************** PRINT TIMING INFORMATION ***************/

    readTime               = 1000.f * (endReadTime - startTime);
    samplingPreprocessTime = 1000.f * (endSamplingPreprocessTime - endReadTime);
    samplingTime           = 1000.f * (endSamplingTime - endSamplingPreprocessTime);
    sampledWriteTime       = 1000.f * (endWriteResampledTime - endSamplingTime);
    segmentPreprocessTime  = 1000.f * (endSegmentPreprocessTime - endWriteResampledTime);
    segmentTime            = 1000.f * (endTime - endSegmentPreprocessTime);
    totalTime              = 1000.f * (endTime - startTime);

    printf("Time for reading input PCD file:              %.4f ms\n", readTime);
    printf("Time for preprocessing PCD for sampling:      %.4f ms\n", samplingPreprocessTime);
    printf("Time for sampling:                            %.4f ms\n", samplingTime);
    printf("Time for writing sampled PCD file:            %.4f ms\n", sampledWriteTime);
    printf("Time for preprocessing PCD for segmentation:  %.4f ms\n", segmentPreprocessTime);
    printf("Time for segmentation:                        %.4f ms\n", segmentTime);
    printf("Total time:                                   %.4f ms\n", totalTime);

    /************* STAGE 5: VISUALIZATION **********/

    // construct segmented point cloud:
    std::vector<int> parents_vec(num_pts_rs);
    std::copy(parents, parents+num_pts_rs,parents_vec.begin());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_seg (new pcl::PointCloud<pcl::PointXYZRGB>);
    constructSegmentedPC(pc_seg,pc_rs,num_pts_rs,parents_vec,flattenXYZ_rs,flattenRGB_rs);

    // write segmented point cloud pc_rs to pcd file
    pcl::io::savePCDFileASCII ("seg_pcd.pcd", *pc_seg);
    std::cerr << "Saved " << pc_seg->size() << " data points to segmented point cloud seg_pcd.pcd." << std::endl;

    return(1);

}


/**
 * @file multicopterDynamicsSim.hpp
 * @author Ezra Tal
 * @brief Multicopter dynamics simulator class header file
 *
 */

#ifndef MULTICOPTERDYNAMICSSIM_H
#define MULTICOPTERDYNAMICSSIM_H

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include "inertialMeasurementSim.hpp"

/**
 * @brief Multicopter dynamics simulator class
 *
 */
class MulticopterDynamicsSim {
 public:
  MulticopterDynamicsSim(int numCopter, double thrustCoefficient,
                         double torqueCoefficient, double minMotorSpeed,
                         double maxMotorSpeed, double motorTimeConstant,
                         double motorRotationalInertia, double vehicleMass,
                         const Eigen::Matrix3d& vehicleInertia,
                         const Eigen::Matrix3d& aeroMomentCoefficient,
                         double dragCoefficient,
                         double momentProcessNoiseAutoCorrelation,
                         double forceProcessNoiseAutoCorrelation,
                         const Eigen::Vector3d& gravity);
  MulticopterDynamicsSim(int numCopter);
  void setVehicleProperties(double vehicleMass,
                            const Eigen::Matrix3d& vehicleInertia,
                            const Eigen::Matrix3d& aeroMomentCoefficient,
                            double dragCoefficient,
                            double momentProcessNoiseAutoCorrelation,
                            double forceProcessNoiseAutoCorrelation);
  void setGravityVector(const Eigen::Vector3d& gravity);
  void setRandomSeed(const unsigned multicopterSeed, const unsigned imuSeed);
  void setMotorFrame(const Eigen::Isometry3d& motorFrame, int motorDirection,
                     int motorIndex);
  void setMotorProperties(double thrustCoefficient, double torqueCoefficient,
                          double motorTimeConstant, double minMotorSpeed,
                          double maxMotorSpeed, double rotationalInertia,
                          int motorIndex);
  void setMotorProperties(double thrustCoefficient, double torqueCoefficient,
                          double motorTimeConstant, double minMotorSpeed,
                          double maxMotorSpeed, double rotationalInertia);
  void setMotorSpeed(double motorSpeed, int motorIndex);
  void setMotorSpeed(double motorSpeed);
  void resetMotorSpeeds(void);
  void setVehiclePosition(const Eigen::Vector3d& position,
                          const Eigen::Quaterniond& attitude);
  void setVehicleState(const Eigen::Vector3d& position,
                       const Eigen::Vector3d& velocity,
                       const Eigen::Vector3d& angularVelocity,
                       const Eigen::Quaterniond& attitude,
                       const std::vector<double>& motorSpeed);
  void getVehicleState(Eigen::Vector3d& position, Eigen::Vector3d& velocity,
                       Eigen::Vector3d& angularVelocity,
                       Eigen::Quaterniond& attitude,
                       std::vector<double>& motorSpeed);
  Eigen::Vector3d getVehiclePosition(void);
  Eigen::Quaterniond getVehicleAttitude(void);
  Eigen::Vector3d getVehicleVelocity(void);
  Eigen::Vector3d getVehicleAngularVelocity(void);

  void proceedState_ExplicitEuler(double dt_secs,
                                  const std::vector<double>& motorSpeedCommand);
  void proceedState_RK4(double dt_secs,
                        const std::vector<double>& motorSpeedCommand);

  void getIMUMeasurement(Eigen::Vector3d& accOutput,
                         Eigen::Vector3d& gyroOutput);

  /// @name IMU simulator
  inertialMeasurementSim imu_ = inertialMeasurementSim(0., 0., 0., 0.);

 private:
  /// @name  Number of rotors
  int numCopter_;

  /// @name Motor properties
  //@{

  // Transform from motor to vehicle (c.o.g.) frame
  /* Motor frame must have prop spinning around z-axis such that
   a positive motor speed corresponds to a positive thrust in
   positive motor z-axis direction.*/
  std::vector<Eigen::Isometry3d> motorFrame_;

  /* +1 if positive motor speed corresponds to positive moment around the motor
     frame z-axis -1 if positive motor speed corresponds to negative moment
     around the motor frame z-axis i.e. -1 indicates a positive motor speed
     corresponds to a positive rotation rate around the motor z-axis
  */
  std::vector<int> motorDirection_;

  std::vector<double> thrustCoefficient_;       // N/(rad/s)^2
  std::vector<double> torqueCoefficient_;       // Nm/(rad/s)^2
  std::vector<double> motorTimeConstant_;       // s
  std::vector<double> motorRotationalInertia_;  // kg m^2
  std::vector<double> maxMotorSpeed_;           // rad/s
  std::vector<double> minMotorSpeed_;           // rad/s
  //@}

  /// @name Vehicle properties
  //@{
  double dragCoefficient_;                         // N/(m/s)
  Eigen::Matrix3d aeroMomentCoefficient_;          // Nm/(rad/s)^2
  double vehicleMass_;                             // kg
  Eigen::Matrix3d vehicleInertia_;                 // kg m^2
  double momentProcessNoiseAutoCorrelation_ = 0.;  // (Nm)^2s
  double forceProcessNoiseAutoCorrelation_ = 0.;   // N^2s
  //@}

  /// @name Std normal RNG
  //@{
  std::default_random_engine randomNumberGenerator_;
  std::normal_distribution<double> standardNormalDistribution_ =
      std::normal_distribution<double>(0.0, 1.0);
  //@}

  // Default reference frame is NED, but can be set by changing gravity
  // direction
  /// @name Gravity vector
  Eigen::Vector3d gravity_;  // m/s^2

  /// @name Vehicle state variables
  //@{
  std::vector<double> motorSpeed_;                             // rad/s
  Eigen::Vector3d velocity_ = Eigen::Vector3d::Zero();         // m/s
  Eigen::Vector3d position_ = Eigen::Vector3d::Zero();         // m
  Eigen::Vector3d angularVelocity_ = Eigen::Vector3d::Zero();  // rad/s
  Eigen::Quaterniond attitude_ = Eigen::Quaterniond::Identity();
  //@}

  /* Vehicle stochastic force vector (in world frame) is maintained
  for accelerometer output, since it must include the same
  random linear acceleration noise as used for dynamics integration*/
  /// @name Vehicle stochastic force vector
  Eigen::Vector3d stochForce_ = Eigen::Vector3d::Zero();  // N

  Eigen::Vector3d getThrust(const std::vector<double>& motorSpeed);
  Eigen::Vector3d getControlMoment(
      const std::vector<double>& motorSpeed,
      const std::vector<double>& motorAcceleration);
  Eigen::Vector3d getAeroMoment(const Eigen::Vector3d& angularVelocity);
  Eigen::Vector3d getDragForce(const Eigen::Vector3d& velocity);
  Eigen::Vector3d getVehicleSpecificForce(void);

  void getMotorSpeedDerivative(std::vector<double>& motorSpeedDer,
                               const std::vector<double>& motorSpeed,
                               const std::vector<double>& motorSpeedCommand);
  Eigen::Vector3d getVelocityDerivative(const Eigen::Quaterniond& attitude,
                                        const Eigen::Vector3d& stochForce,
                                        const Eigen::Vector3d& velocity,
                                        const std::vector<double>& motorSpeed);
  Eigen::Vector3d getAngularVelocityDerivative(
      const std::vector<double>& motorSpeed,
      const std::vector<double>& motorAcceleration,
      const Eigen::Vector3d& angularVelocity,
      const Eigen::Vector3d& stochMoment);
  Eigen::Vector4d getAttitudeDerivative(const Eigen::Quaterniond& attitude,
                                        const Eigen::Vector3d& angularVelocity);
  void vectorAffineOp(const std::vector<double>& vec1,
                      const std::vector<double>& vec2,
                      std::vector<double>& vec3, double val);
  void vectorScalarProd(const std::vector<double>& vec1,
                        std::vector<double>& vec2, double val);
  void vectorBoundOp(const std::vector<double>& vec1, std::vector<double>& vec2,
                     const std::vector<double>& minvec,
                     const std::vector<double>& maxvec);
};

#endif  // MULTICOPTERDYNAMICSSIM_H


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/constants/eps_related.hpp>
#include <nt2/include/functions/splat.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include "../constant.hpp"

// floating point underflow when inputting Mindenormal value
#ifdef __ICC
#pragma warning(disable: 239)
#endif

NT2_TEST_CASE_TPL( eps_value, BOOST_SIMD_SIMD_TYPES )
{
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef boost::simd::native<T,ext_t>  n_t;

  NT2_CHECK_CONSTANT(Eps            , nt2::Eps<T>()           , n_t);
  NT2_CHECK_CONSTANT(Halfeps        , nt2::Halfeps<T>()       , n_t);
  NT2_CHECK_CONSTANT(Threeeps       , nt2::Threeeps<T>()      , n_t);
  NT2_CHECK_CONSTANT(Sqrteps        , nt2::Sqrteps<T>()       , n_t);
  NT2_CHECK_CONSTANT(Fourthrooteps  , nt2::Fourthrooteps<T>() , n_t);
  NT2_CHECK_CONSTANT(Thirdrooteps   , nt2::Thirdrooteps<T>()  , n_t);
  NT2_CHECK_CONSTANT(Mlogeps2       , nt2::Mlogeps2<T>()      , n_t);
  NT2_CHECK_CONSTANT(Smallestposval , nt2::Smallestposval<T>(), n_t);
  NT2_CHECK_CONSTANT(Sqrtsmallestposval , nt2::Sqrtsmallestposval<T>(), n_t);
  NT2_CHECK_CONSTANT(Mindenormal    , nt2::Mindenormal<T>()   , n_t);
}


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


// Boost.Geometry
// Unit Test

// Copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <geometry_test_common.hpp>

#include <iostream>

#include <boost/geometry/srs/projection.hpp>

#include "projection_selftest_cases.hpp"
#include "proj4.hpp"

void test_projection(std::string const& id, std::string const& parameters,
                     const LL * fwd_in, const XY * fwd_expected,
                     const XY * inv_in, const LL * inv_expected)
{
    bg::srs::projection<> prj = bg::srs::proj4(parameters);

#ifdef TEST_WITH_PROJ4
    pj_projection pj_par(parameters);
#endif

    for (std::size_t i = 0 ; i < 4 ; ++i)
    {
        if (bg::get<0>(fwd_expected[i]) == HUGE_VAL)
            break;

        {
            XY fwd_out;
            prj.forward(fwd_in[i], fwd_out);

            bool fwd_eq = bg::math::abs(bg::get<0>(fwd_out) - bg::get<0>(fwd_expected[i])) < 1e-7
                       && bg::math::abs(bg::get<1>(fwd_out) - bg::get<1>(fwd_expected[i])) < 1e-7;

            BOOST_CHECK_MESSAGE((fwd_eq),
                                std::setprecision(16) << "Result of " << id << " forward projection {"
                                << bg::wkt(fwd_out) << "} different than expected {"
                                << bg::wkt(fwd_expected[i]) << "}");

#ifdef TEST_WITH_PROJ4
            {
                XY pj_xy;
                pj_par.forward(fwd_in[i], pj_xy);
                double d1 = bg::math::abs(bg::get<0>(fwd_out) - bg::get<0>(pj_xy));
                double d2 = bg::math::abs(bg::get<1>(fwd_out) - bg::get<1>(pj_xy));
                double d = (std::max)(d1, d2);
                bool same_as_pj = d < 1e-15;
                BOOST_CHECK_MESSAGE((same_as_pj),
                                    std::setprecision(16) << "Result of " << id << " forward projection {"
                                    << bg::wkt(fwd_out) << "} different than Proj4 {"
                                    << bg::wkt(pj_xy) << "} by " << d);
            }
#endif
        }

        if (bg::get<0>(inv_expected[i]) == HUGE_VAL)
            break;

        {
            LL inv_out;
            prj.inverse(inv_in[i], inv_out);

            bool inv_eq = bg::math::abs(bg::get<0>(inv_out) - bg::get<0>(inv_expected[i])) < 1e-7
                       && bg::math::abs(bg::get<1>(inv_out) - bg::get<1>(inv_expected[i])) < 1e-7;

            BOOST_CHECK_MESSAGE((inv_eq),
                                std::setprecision(16) << "Result of " << id << " inverse projection {"
                                << bg::wkt(inv_out) << "} different than expected {"
                                << bg::wkt(inv_expected[i]) << "}");

#ifdef TEST_WITH_PROJ4
            {
                LL pj_ll;
                pj_par.inverse(inv_in[i], pj_ll);
                double d1 = bg::math::abs(bg::get<0>(inv_out) - bg::get<0>(pj_ll));
                double d2 = bg::math::abs(bg::get<1>(inv_out) - bg::get<1>(pj_ll));
                double d = (std::max)(d1, d2);
                bool same_as_pj = d < 1e-15;
                BOOST_CHECK_MESSAGE((same_as_pj),
                                    std::setprecision(16) << "Result of " << id << " inverse projection {"
                                    << bg::wkt(inv_out) << "} different than Proj4 {"
                                    << bg::wkt(pj_ll) << "} by " << d);
            }
#endif
        }
    }
}

void test_projections(const projection_case * cases, std::size_t n)
{
    for (std::size_t i = 0 ; i < n ; ++i)
    {
        projection_case const& pcas = cases[i];

        test_projection(pcas.id, pcas.args,
                        pcas.fwd_in, pcas.fwd_expect,
                        pcas.inv_in, pcas.inv_expect);
    }
}

int test_main(int, char*[])
{
    test_projections(projection_cases, sizeof(projection_cases)/sizeof(projection_case));

    return 0;
}


/*
Olalekan Ogunmolu.
SeRViCe Lab,
Feb. 18, 2017*/

#include "ros/ros.h"
#include <ros/spinner.h>
#include "std_msgs/String.h"
#include <ros/console.h>
#include <tf/transform_broadcaster.h>
#include <thread>
#include <string>
#include <fstream>
#include <mutex>
#include <chrono>

#include <vicon_bridge/Markers.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>

#include <tf/transform_datatypes.h>
#include <tf_conversions/tf_eigen.h>

#include "boost/bind.hpp"
#include <boost/thread.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>

const std::string Superchick_name = "Superchicko";
std::string listener = "vicon_listener";

std::string subject, segment;
const std::string globalTopicName = "/vicon/Superdude/head";
using namespace Eigen;

class Receiver
{
private:
    float xm, ym, zm;
    bool save, print, sim, running, firstIter;
    double headRoll, headPitch, headYaw;
    double panelRoll, panelPitch, panelYaw;
    int count;

    Vector3d rpy;

    std::vector<geometry_msgs::Point> headMarkersVector,
                                      panelMarkersVector;

    std::vector<std::thread> threads;
    ros::NodeHandle nm_;
    ros::Publisher pub;
    Matrix3d R; //basis rotation matrix with respect to camera
    boost::mutex rotation_mutex_, markers_mutex;
    geometry_msgs::Vector3 headTrans;
    geometry_msgs::Quaternion headQuat;
    std::mutex mutex;
    bool updatePose;
    ros::AsyncSpinner spinner;
    unsigned long const hardware_threads;

    //for rigidTransforms
    geometry_msgs::Vector3 panelTrans;
    geometry_msgs::Quaternion panelQuat;
    //pose vector
    geometry_msgs::Pose pose;
    ros::Publisher pose_pub_;

    std::vector<std::thread> threadsVector;
    std::thread testQuatThread, modGramScmidtThread,
                rotationMatrixThread;

    Matrix3d headMGS, tableMGS, rotationMatrix;

    //use exactTimePolicy to process panel markers and head markers
    // using namespace message_filters;

    using vicon_sub = message_filters::Subscriber<vicon_bridge::Markers> ;
    using head_sub  = message_filters::Subscriber<geometry_msgs::TransformStamped>;
    using pane_sub  = message_filters::Subscriber<geometry_msgs::TransformStamped>;
    using headSyncPolicy = message_filters::sync_policies::ExactTime<vicon_bridge::Markers,
                                geometry_msgs::TransformStamped, geometry_msgs::TransformStamped>;

    vicon_sub subVicon;
    head_sub  subHead;
    pane_sub  subPanel;

    message_filters::Synchronizer<headSyncPolicy> sync;

public:
    Receiver(ros::NodeHandle nm, bool save, bool print, bool sim)
    :  nm_(nm), save(save), print(print), sim(sim), hardware_threads(std::thread::hardware_concurrency()),
       subVicon(nm_, "/vicon/markers", 1), subHead(nm_, "vicon/Superdude/head", 1),
       subPanel(nm_,"/vicon/Panel/rigid", 1), spinner(2), count(0), firstIter(false),
       sync(headSyncPolicy(10), subVicon, subHead, subPanel), updatePose(false)
    {
        // ExactTime takes a queue size as its constructor argument, hence SyncPolicy(10)
       sync.registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3));
       pose_pub_ = nm_.advertise<geometry_msgs::Pose>("/mannequine_head/pose", 10);
    }

    ~Receiver()
    { }

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

    void run()
    {
      spawn();
      unspawn();
    }
private:
    void spawn()
    {
        if(spinner.canStart())
            spinner.start();
        running = true;

        while(!updatePose)
            std::this_thread::sleep_for(std::chrono::milliseconds(1));

        //spawn the threads
        modGramScmidtThread = std::thread(&Receiver::modGramSchmidt, this);
        // rotationMatrixThread = std::thread(&Receiver::computeRotationMatrix,
                                                             // this);

        if(modGramScmidtThread.joinable())
            modGramScmidtThread.join();
        // if(rotationMatrixThread.joinable())
            // rotationMatrixThread.join();
    }

    void unspawn()
    {
        spinner.stop();
        modGramScmidtThread.detach();
        // rotationMatrixThread.detach();
        running = false;
    }

    void callback(const vicon_bridge::MarkersConstPtr& markers_msg,
                const geometry_msgs::TransformStampedConstPtr& panel_msg,
                const geometry_msgs::TransformStampedConstPtr& head_msg)
    {
        //solve all vicon markers here
        //Retrieve geometry_msgs translation for four markers on superchicko
        std::vector<geometry_msgs::Point> headMarkersVector;
        headMarkersVector.resize(4);
        headMarkersVector[0] = markers_msg -> markers[0].translation;  //fore
        headMarkersVector[1] = markers_msg -> markers[1].translation;   //left
        headMarkersVector[2] = markers_msg -> markers[2].translation;   //chin
        headMarkersVector[3] = markers_msg -> markers[3].translation;   //right

        std::vector<geometry_msgs::Point> panelMarkersVector;
        panelMarkersVector.resize(4);
        panelMarkersVector[0] = markers_msg->markers[4].translation;    //tabfore
        panelMarkersVector[1] = markers_msg->markers[5].translation;    //tableft
        panelMarkersVector[2] = markers_msg->markers[6].translation;    //tabchin
        panelMarkersVector[3] = markers_msg->markers[7].translation;    //tabright

        //solve head transformedstamp markers here
        geometry_msgs::Vector3 headTrans = head_msg->transform.translation;
        geometry_msgs::Quaternion headQuat = head_msg->transform.rotation;

        //solve panel transformedstamp markers here
        geometry_msgs::Vector3 panelTrans = panel_msg->transform.translation;
        geometry_msgs::Quaternion panelQuat = panel_msg->transform.rotation;

        //convert translations to millimeters
        metersTomilli(std::move(headTrans));
        metersTomilli(std::move(panelTrans));

        boost::mutex::scoped_lock lock(markers_mutex);
        this->headTrans   = headTrans;
        this->headQuat    = headQuat;

        this->panelTrans    = panelTrans;
        this->panelQuat  = panelQuat;

        this->headMarkersVector = headMarkersVector;
        this->panelMarkersVector= panelMarkersVector;

        updatePose          = true;
        lock.unlock();
    }

    void modGramSchmidt() noexcept
    {
        std::vector<geometry_msgs::Point> headMarkersVector,
                                          panelMarkersVector;

        //attached frame to face
        Vector3d fore, left, right, chin;
        //base markers
        Vector3d tabfore, tableft, tabright, tabchin;

        Matrix3d headMGS, tableMGS;

        Vector3d rpy;
        ros::Rate looper(10);

        for(; running && ros::ok() ;)
        {
            if(updatePose)
            {
                boost::mutex::scoped_lock lock(markers_mutex);
                headMarkersVector  = this->headMarkersVector;
                panelMarkersVector = this->panelMarkersVector;
                updatePose = false;
                lock.unlock();
            }

            fore << headMarkersVector[0].x, headMarkersVector[0].y,
                    headMarkersVector[0].z;
            left << headMarkersVector[1].x, headMarkersVector[1].y,
                    headMarkersVector[1].z;
            chin << headMarkersVector[2].x, headMarkersVector[2].y,
                    headMarkersVector[2].z;
            right << headMarkersVector[3].x, headMarkersVector[3].y,
                    headMarkersVector[3].z;

            tabfore << panelMarkersVector[0].x, panelMarkersVector[0].y,
                       panelMarkersVector[0].z;
            tableft << panelMarkersVector[1].x, panelMarkersVector[1].y,
                       panelMarkersVector[1].z;
            tabchin << panelMarkersVector[2].x, panelMarkersVector[2].y,
                       panelMarkersVector[2].z;
            tabright << panelMarkersVector[3].x, panelMarkersVector[3].y,
                       panelMarkersVector[3].z;

            //subtract the markers to create axes
            left     -= right;
            fore     -= left;
            //table markers
            tableft -= tabright;
            tabfore  -= tableft;
            // botRight -= topRight;
            //define the unit axis vectors kx, ky, kz for head frame
            Vector3d kx, ky, kz;

            /*adapted from Teddy's code
            * Gets the orientation quaternion of a planar model such that the
            * z-axis is normal to the plane, and the x-y
            * axis are as close as possible to the the table frame
            */
            //http://answers.ros.org/question/31006/how-can-a-vector3-axis-be-used-to-produce-a-quaternion/
            Vector3d item_z((left(0)+right(0)+fore(0)+chin(0))/4,
                            (left(1)+right(1)+fore(1)+chin(1))/4,
                            (left(2)+right(2)+fore(2)+chin(2))/4);
            item_z.normalize();

            Vector3d table_z(0.0, 0.0, 1.0);
            // z_vector.normalize();

            double normAcrossB = item_z.cross(table_z).norm(); // this gives us A cross B norm
            double normBcrossA = table_z.cross(item_z).norm(); // this gives us B cross A norm
            double AdotB = item_z.dot(table_z);

            Matrix3d G; // this is matrix G for manipulation
            G <<    AdotB, -1*normAcrossB, 0,
                    normAcrossB, AdotB, 0,
                    0, 0, 1;

            //Calc U
            Vector3d U = (AdotB * item_z).normalized();
            Vector3d V = (table_z - AdotB * item_z).normalized();
            Vector3d W = table_z.cross(item_z);
            Matrix3d Fproto;
            Fproto << U, V, W;

            Matrix3d Rotation_Matrix = (Fproto * G * Fproto.inverse()).inverse();
            // Quaterniond q(Rotation_Matrix);
            // tf::Quaternion tf_q;
            // tf::quaternionEigenToTF(q, tf_q);

            // tf::Vector3 y_vector = axis_vector.cross(z_vector);
            kx = fore.cross(chin)/(fore.cross(chin)).norm();
            ky = left.cross(right)/(left.cross(right)).norm();
            kz = kx.cross(ky);
            //define the unit axes vectors kxp, kyp, kzp forthe table frame'
            Vector3d kxp, kyp, kzp;
            kxp = tabfore.cross(tabchin)/(tabfore.cross(tabchin)).norm();
            kyp = tableft.cross(tabright)/(tableft.cross(tabfore)).norm();
            kyp = kxp.cross(kyp);

            //see https://ocw.mit.edu/courses/mathematics/18-335j-introduction-to-numerical-methods-fall-2010/lecture-notes/MIT18_335JF10_lec10a_hand.pdf
            std::vector<Vector3d> v(4), q(4);
            //compute orthonormal basis vectors for face markers
            // v[0] = kx;   v[1] = ky;   v[2] = kz;
            v[0] = left; v[1] = right; v[2] = left.cross(right);
            double r[3][3] = {};
            for(auto i = 0; i < 3; ++i)
            {
                r[i][i] = v[i].norm();
                q[i]    = v[i]/r[i][i];
                for(auto j = i +1; j < 3; ++j)
                {
                    r[i][j] = q[i].transpose() * v[j];
                    v[j]    = v[j] - r[i][j] * q[i];
                }
            }

            // populate headMGS rotation basis matrix
            for(auto i =0; i < 3; ++i)
            {
                headMGS.col(i) = q[i];
            }
            this->headMGS = headMGS;

            //compute basis vectors for table markers
            v.clear(); q.clear();
            v[0] = kxp; v[1] = kyp; v[2] = kzp;
            v[0] = tableft; v[1] = tabright; v[2] = tableft.cross(tabright);
            for(auto i = 0; i < 3; ++i)
            {
                r[i][i] = v[i].norm();
                q[i]    = v[i]/r[i][i];
                for(auto j = i +1; j < 4; ++j)
                {
                    r[i][j] = q[i].transpose() * v[j];
                    v[j]    = v[j] - r[i][j] * q[i];
                }
            }
            // populate tableMGS rotation basis matrix
            for(auto i =0; i < 3; ++i)
            {
                tableMGS.col(i) = q[i];
            }
            this->tableMGS = tableMGS;

            /*
            The rotation matrix of the head with respect to the table follows
            John Craig's Introduction to Robotics Book convention[p.22] and it
            is denoted by

            TRH = [ TXH    TYH     TZH ]   (H denotes head)

                = [XH.XT  YH.XT   ZH.XT]    (T denotes table frame)
                  [XH.YT  YH.YT   ZH.YT]
                  [XH.ZT  YH.ZT   ZH.ZT]
            where
            H   = [XH  YH  ZH]
            T   = [XT  YT  ZT]
            */
            //First Row
            rotationMatrix(0, 0) = headMGS.col(0).dot(tableMGS.col(0));
            rotationMatrix(0, 1) = headMGS.col(1).dot(tableMGS.col(0));
            rotationMatrix(0, 2) = headMGS.col(2).dot(tableMGS.col(0));
            //Second Row
            rotationMatrix(1, 0) = headMGS.col(0).dot(tableMGS.col(1));
            rotationMatrix(1, 1) = headMGS.col(1).dot(tableMGS.col(1));
            rotationMatrix(1, 2) = headMGS.col(2).dot(tableMGS.col(1));
            //Third Row
            rotationMatrix(2, 0) = headMGS.col(0).dot(tableMGS.col(2));
            rotationMatrix(2, 1) = headMGS.col(1).dot(tableMGS.col(2));
            rotationMatrix(2, 2) = headMGS.col(2).dot(tableMGS.col(2));

            this->rotationMatrix = rotationMatrix;
            // rollpy(Rotation_Matrix);
            rollpy(rotationMatrix);

            // std::cout << "\nheadMGS: \n" << headMGS << std::endl;
            // std::cout << "tableMGS: \n" << tableMGS << std::endl;
            // std::cout <<"rpy in mgs: " << this->rpy.transpose() << std::endl;

            pose.position.x = item_z(0) * 1000;
            pose.position.y = item_z(1) * 1000;
            pose.position.z = item_z(2) * 1000;
            pose.orientation.x = this->rpy(0); //roll
            pose.orientation.y = this->rpy(1); //pitch
            pose.orientation.z = this->rpy(2); //yaw
            pose.orientation.w = 1.0;
            pose_pub_.publish(pose);

            looper.sleep();
        }
    }

    //From Rotation Matrix, find rpy
    void rollpy(Matrix3d R) //const
    {
        Vector3d rpy;
        if (  (std::fabs(R(0,0)) < .001) && (std::fabs(R(1,0)) < .001) )
        {
            // singularity
            rpy(0) = 0;
            rpy(1) = std::atan2(-R(2,0), R(0,0));
            rpy(2) = std::atan2(-R(1,2), R(1,1));
        }
        else
        {
            rpy(0) = std::atan2(R(1,0), R(0,0));
            rpy(1) = std::atan2(
                                -R(2,0), (
                                         (std::cos(rpy(0)) * R(0,0)) +
                                         (std::sin(rpy(0)) * R(1,0))
                                         )
                                );
            rpy(2) = std::atan2(
                                (std::sin(rpy(0)) * R(0,2) - std::cos(rpy(0)) * R(1,2)),
                                (std::cos(rpy(0))*R(1,1) - std::sin(rpy(0))*R(0,1))
                                );
        }
        //convert to degree
        rad2deg(std::forward<Vector3d>(rpy));
        // ROS_INFO_STREAM("rpy: " << rpy.transpose());
        this->rpy = rpy;
    }

    void testQuat() noexcept
    {
        geometry_msgs::Quaternion headQuat, panelQuat;
        // if(updatePose)
        // {

        boost::mutex::scoped_lock lock(markers_mutex);
        // std::lock_guard<std::mutex> lock(mutex);
        {
            headQuat = this->headQuat;
            panelQuat = this->panelQuat;
            updatePose = false;
        }
        lock.unlock();
        // }
        double roll, rolla, pitch, pitcha, yaw, yawa;

        getRPYFromQuaternion(std::forward<geometry_msgs::Quaternion>(headQuat),
                             std::forward<double>(roll),
                             std::forward<double>(pitch),
                             std::forward<double>(yaw));

        // rad2deg(std::move(roll));
        // rad2deg(std::move(pitch));
        // rad2deg(std::move(yaw));

        printf("\n|\troll \t|\tpitch \t|\tyaw\n %f , \t%f, \t%f",
                        roll, pitch, yaw);

    }

    void getRPYFromQuaternion(geometry_msgs::Quaternion&& rotQuat, double&& roll,
                             double&& pitch, double&& yaw)
    {

        tf::Quaternion q(rotQuat.x, rotQuat.y, rotQuat.z, rotQuat.w);
        tf::Matrix3x3 m(q);
        m.getRPY(roll, pitch, yaw);
        rad2deg(std::move(roll)); rad2deg(std::move(pitch)); rad2deg(std::move(yaw));
    }

    inline void metersTomilli(geometry_msgs::Vector3&& translation)
    {
        translation.x   *= 1000;
        translation.y   *= 1000;
        translation.z   *= 1000;
    }

    inline void rad2deg(double&& x)
    {
        x  *= 180/M_PI;
    }

    inline void rad2deg(Vector3d&& x)
    {
        rad2deg(std::move(x(0)));
        rad2deg(std::move(x(1)));
        rad2deg(std::move(x(2)));
    }

    void savepoints()
    {
        //Now we write the points to a text file for visualization processing
        std::ofstream midface;
        midface.open("midface.csv", std::ofstream::out | std::ofstream::app);
        midface << xm <<"\t" <<ym << "\t" << zm << "\n";
        midface.close();
    }
};

int main(int argc, char **argv)
{

    uint32_t options = 0;

    ros::init(argc, argv, listener, ros::init_options::AnonymousName);

    bool save, print, sim;

    ros::NodeHandle nm;

    save = nm.getParam("save", save) ;
    print = nm.getParam("print", print);
    sim = nm.getParam("sim", sim);

    Receiver  r(nm, save, print, sim);
    r.run();

    if(!ros::ok())
    {
      return 0;
    }

    ros::shutdown();
}


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/string.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {

{

//! [BOOST_HANA_STRING]
BOOST_HANA_CONSTEXPR_LAMBDA auto str = BOOST_HANA_STRING("abcdef");
static_assert(std::is_same<datatype_t<decltype(str)>, String>{}, "");
//! [BOOST_HANA_STRING]

}{

//! [string]
constexpr auto str = string<'a', 'b', 'c', 'd', 'e', 'f'>;
static_assert(std::is_same<datatype_t<decltype(str)>, String>{}, "");
//! [string]

}{

//! [comparable]
BOOST_HANA_CONSTANT_CHECK(
    BOOST_HANA_STRING("abcdef") == BOOST_HANA_STRING("abcdef")
);

BOOST_HANA_CONSTANT_CHECK(
    BOOST_HANA_STRING("abcdef") != BOOST_HANA_STRING("abef")
);
//! [comparable]

}{

//! [orderable]
BOOST_HANA_CONSTANT_CHECK(
    BOOST_HANA_STRING("abc") < BOOST_HANA_STRING("bcd")
);

BOOST_HANA_CONSTANT_CHECK(
    BOOST_HANA_STRING("abcd") > BOOST_HANA_STRING("abc")
);
//! [orderable]

}{

//! [foldable]
auto sum_string = [](auto str) {
    return foldl(str, int_<0>, [](auto sum, auto c) {
        constexpr int i = value(c) - 48; // convert character to decimal
        return sum + int_<i>;
    });
};

BOOST_HANA_CONSTANT_CHECK(
    sum_string(BOOST_HANA_STRING("1234")) == int_<1 + 2 + 3 + 4>
);
//! [foldable]

}{

//! [iterable]
BOOST_HANA_CONSTANT_CHECK(!is_empty(BOOST_HANA_STRING("abcd")));
BOOST_HANA_CONSTANT_CHECK(is_empty(BOOST_HANA_STRING("")));

BOOST_HANA_CONSTANT_CHECK(BOOST_HANA_STRING("abcd")[int_<2>] == char_<'c'>);

auto is_vowel = [](auto c) {
    return c ^in^ BOOST_HANA_STRING("aeiouy");
};
BOOST_HANA_CONSTANT_CHECK(
    drop_while(BOOST_HANA_STRING("aioubcd"), is_vowel) == BOOST_HANA_STRING("bcd")
);
//! [iterable]

}{

//! [searchable]
BOOST_HANA_CONSTANT_CHECK(char_<'c'> ^in^ BOOST_HANA_STRING("abcde"));
BOOST_HANA_CONSTANT_CHECK(!(char_<'z'> ^in^ BOOST_HANA_STRING("abcde")));

BOOST_HANA_CONSTANT_CHECK(
    find(BOOST_HANA_STRING("abcxefg"), char_<'x'>) == just(char_<'x'>)
);
//! [searchable]

}

}


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

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

#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/definition/trunc.hpp>
#include <boost/simd/function/definition/fix.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/dispatch/hierarchy.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{

  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( idiv_
                          , (typename T)
                          , bd::cpu_
                          , bd::generic_<bd::arithmetic_<T>>
                          , bd::generic_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE  bd::as_integer_t<T>
    operator()(T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return div(a, b, fix);
    }
  };
//   BOOST_DISPATCH_OVERLOAD ( idiv_
//                           , (typename T)
//                           , bd::cpu_
//                           , bd::generic_<bd::arithmetic_<T>>
//                           , bd::generic_<bd::arithmetic_<T>>
//                           , bs::tag::trunc_
//                           )
//   {
//     BOOST_FORCEINLINE bd::as_integer_t<T>
//     operator()(T const& a, T const& b
//                , bd::functor<bs::tag::trunc_> ) const BOOST_NOEXCEPT
//     {
//       return idiv(a, b, fix);
//     }
//   };

} } }

#endif